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ABSTRACT 


OBJECT LOCALIZATION AND ITS APPLICATIONS 
IN TELE-AUTONOMOUS SYSTEMS 


by 


Lejun Shao 


Chairman: Richard A. Volz, Michael W. Walker 


Object localization and its applications in tele-autonomous systems are studied in this 
thesis. 

The thesis first gives a thorough investigation of the object localization problem and 
then presents two object localization algorithms together with the methods of extracting 
several important types of object features. The first algorithm is based on line-segment to 
line-segment matching. Line range sensors are used to extract line-segment features from 
an object, the features may be boundary edges of planar surfaces, the axes of cylindrical 
surfaces, conic surfaces, or other types of surfaces of revolution. The extracted features 
are matched to corresponding model features to compute the location of the object. The 
second algorithm is more general. The inputs of the algorithm are not limited only to 



the line features. Featured points (point-to-point matching) and featured unit direction 
vectors (vector-to-vector matching) can also be used as the inputs of the algorithm, and 
there is no upper limit on the number of the features inputed. The algorithm will allow 
the use of redundant features to find a better solution. The algorithm uses dual number 
quaternions to represent the position and orientation of an object and uses the least 
squares optimization method to find an optimal solution for the object’s location. The 
advantage of using this representation is that the method solves for the location estimation 
by minimizing a single cost function associated with the sum of the orientation and position 
errors and thus has a better performance on the estimation, both in accuracy and in speed, 
than that of other similar algorithms. 

The thesis discusses the difficulties when an operator is controlling a remote robot 
to perform manipulation tasks. The main problems facing the operator are time-delays 
on the signal transmission and the uncertainties of the remote environment. How object 
localization techniques can be used together with other techniques such as predictor display 
and time desynchronization to help to overcome these difficulties are then discussed. The 
thesis discusses two cases where object localization can help: 1) the case where direct 
manual control is used to perform a tele-manipulation task; 2) the case where the remote 
system has certain degree of automation ability. 
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CHAPTER 1 


Introduction 


1.1 Background 

Until now, direct manual control or teleoperation has been the dominant technique to tele- 
control the operation of a remote robot, in which all the intellectual inputs are from the 
human operator in a control center distant from the workspace. The techniques have been 
mainly applied to underwater operations and nuclear industry in circumstances hazardous 
to human beings. There are many remote manipulators being used in nuclear industry 

today [41]. 

Recently, research scientists have put great efforts to develop telerobotic technology for 
space applications. NASA, in particular, has carried out considerable work in developing 
telerobotic systems over recent years [4] [55], [65], [67], [81]. The direct motivations have 
been the Congress’s legislation on the construction of a permanent Space Station which is 
scheduled to be put into orbit in the middle of the 1990’s; and the legislation that no less 
than ten percent of the total Space Station costs must be used in developing automation 
and robotics technology [27]. For this purpose, [84] has described an early view of NASA’s 
automation and robotics technology development program. The objective of the program 
is to improve the productivity, capability, autonomy, and safety of future space missions. 
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Other factors also contributed to the increasing research activity in this area such as the 
strong desire to replace humans for the performance of hazardous, strenuous, or repetitive 
tasks and so on. Several important types of tasks have been identified as the candidates 
for telerobot applications, including: Space Station construction, maintenance on the 
Space Station, remote satellite servicing etc. [84]. To achieve success in all the work just 
mentioned, however, the present technology must be evolved from the current level to a 
much higher level. According to Iyengar and Kashyap [60] the evolution of autonomy can 
be described as the following four different levels with increasing intelligence and difficulty: 

• teleoperation - direct manual control of the remote manipulator; 

. telesensing, or telepresence - human operator will feel physically present at the remote site 
by receiving remote sensing information. 

. telerobotics, or supervisory control - the human operator acts merely as a supervisor, in- 
termittently communicating to the remote system, giving suggestions, changing orders and 

etc* 

• intelligent autonomous robots - the human operator supplies a single high-level command, 
in response to which the robot does all the necessary tasks. 

The technologies needed to reach the goal of intelligent autonomous robots have been 
identified. Among other things, such as real-time path planning, the design of dexterous 
manipulator, adaptive control, etc., the vision capability is definitely a very important one. 
The research under this category includes the recognition, localization and the inspection 
of objects, optimal sensor configuration, e.g., sensor mounting requirements, hierarchical 
sensing with varying degree of coarseness, integration of sensing data, the development of 
high-speed and high-quality sensing hardware and software, etc.. The object localization 
problem is the topic of this thesis. 

Object localization refers to the problem of determining the location (position and 
orientation) of a known object in three dimensional space. Before a manipulator can 
do any operation on an object, either grasping the object, or inserting the object into 
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another one, the manipulator must know the location of the object(s). In an industrial 
environment, especially in large scale batch manufacturing environment, the manipulators 
are pre-programmed. A set of fixture and jigs are provided to guarantee the exact location 
of the object or to limit the uncertainty on the object’s location to a tolerable degree so that 
the manipulator’s operations would not fail. In space applications, however, the situation 
is totally different. Most of the tasks are small scale and thus require the flexibility of the 
remote manipulator. One of the flexibilities the remote manipulator should have is the 
ability to locate an object and adjust its position accordingly. 

1.2 Research Goal 

Automatic object localization has attracted increasing attention recently in both com- 
puter vision and robotics communities because researchers and scientist in these fields 
have found the important role this technique will play in developing autonomous intelli- 
gent machines. On the other hand, as an independent research topic, only few research 
work on object localization can be found in literature. Often the object localization prob- 
lem is combined into object recognition research and the emphasis of the research in most 
cases is on object recognition. But object localization has many issues of its own and 
the recognition-localization is only one strategy toward this problem as will be seen in 

Chapter 3. 

The objective of the thesis is to investigate methods of object localization, i.e., given a 
known object, find its position and orientation. The methods explored are intended to be 
used in tele-autonomous applications, especially in space programs such as Space Station 
construction, maintenance and repairing. But, the results obtained should be applicable 
to other related areas such as flexible robotic assembly, inspection and etc.. The methods 
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explored in this thesis should meet the following requirements: 

1. They should be fast. A complete localization process should be finished in seconds; 

2. They are accurate so the results of localization can be used in following telerobotic 

operations reliably; 

3. They can locate most types of the industrial parts so the methods developed in this 
thesis will have real practical meaning. 

Based on these requirements, the thesis addresses the following problems: 

1. What degree of accuracy is required in tele-manipulation tasks? Furthermore, if 
the accuracy requirement for a particular tele- manipulation task is given and other 
resources of potential position errors during the execution of the task can also be 
identified, what degree of object localization accuracy should be provided by the 
sensing system? 

2. What types of range sensing techniques are good for fast localization? 

3. Are there better algorithms or mathematical methods which would further increase 
the speed and efficiency of the computations needed in localization process? Can the 
same computation be carried out by using closed form formula instead of iterative 
methods without losing accuracy? 

4. Is the algorithm sensitive to noise? 

Two localization algorithms are proposed in the thesis. The extraction of certain types of 
features such as line-segment features is also described. All the algorithms and methods 
explored are directed toward the goal of fast, accurate and robust object localization. 
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1.3 The Overall Organization of the Thesis 


This thesis discusses the issues of object localization and its applications in a tele- autonomous 
robot. 

Chapter 2 gives a detailed discussion of the applications of object localization tech- 
nique to tele-autonomous systems. It begins by a brief review of the development of 
tele-autonomous system concepts and implementation. The difficulties in performing tele- 
manipulation tasks are then described, which include time-delay problem and the various 
uncertainties in the remote environment. How object localization techniques can be used 
to help the successful completion of these tele-manipulation tasks are outlined. Two sit- 
uations are discussed: 1) the situations where the tele-manipulation tasks are performed 
manually, e.g., local operator directly controls all the operations of a remote manipulator; 
2) the situations of limited autonomous control where the remote system has a certain 
degree of automation ability. 

A thorough investigation of object localization problems is presented in Chapter 3. 
This includes a formulation of the object localization problem; a description of the overall 
organization of object localization systems; a classification of object localization strategies 
and a discussion of important issues related to the quality of object location determi- 
nation such as ranging techniques, speed and accuracy consideration, types of locatable 
objects, mathematical formulation methods and etc. Arguments for using a line based 
range sensing system for a fast and accurate object localization are presented. 

Chapters 4 and 5 present a detailed discussion of object localization techniques. Chap- 
ter 4 deals with the line-segment matching based localization method. It consists of three 
parts. The first part discusses the process of using line range data to extract certain types 



6 


of features. This includes the extraction of edge parameters from a planar surface, the 
extraction of axis parameters from a surface of revolution and the extraction of the center 
point from a sphere. The performance of the extraction algorithms with data corrupted 
by noise are discussed. The second part presents the mathematical formulation of the 
line-segment matching localization algorithm, which includes the cases that the two line- 
segments are in the same plane and they are in different planes, and a discussion of the 

sensitivity analysis of the algorithm. 

Chapter 5 discusses a more general case where, in addition to line-segment features, 
other features such as featured points, surface normals, etc. are also available and the 
number of available features might be more than the minimum required. An optimal 
localization algorithm using dual quaternions is presented. The advantages of using this 
algorithm, compared with other similar algorithms, are its high accuracy, fast execution 

speed and flexibility. 

Experimental results both from computer simulation and from real range data are 
provided in the last part of each of the two chapters, which confirmed these algorithms’ 

high-speed and high-accuracy performance. 

Chapter 6 describes techniques for computing the location of a line sensor relative to its 
mounting position, particularly when the sensor is mounted somewhere on a joint of a tele- 
robot. This refers to the sensor calibration problem. Both internal calibration and external 
calibration are discussed. For the internal calibration, we only deal with the calibration 
method of the line sensor which is used in our experiments. The external calibration 
methods are more general and the solutions are given for three different situations: 1) 
an object used for the calibration purpose can be accurately placed in a pre-determined 
position and the sensor can locate the object from a single frame of measurement; 2) the 
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object’s position is unknown but the sensor is able to locate it by using a single frame of 
measurement; 3) the object’s location is unknown and the sensor can only locate certain 

features from a single frame of measurement. 

Chapter 7 contains a discussion of what additional work has to be done in order for the 
technique discussed in previous chapters to be used in practical tele-autonomous systems. 
The potential areas of future research are identified with a conclusion being given in the 


last. 
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CHAPTER 2 


APPLICATIONS OF OBJECT LOCALIZATION IN 
TELE-AUTONOMOUS SYSTEMS 

In this chapter, we present a discussion of the problems of tele-controlling the movement 
of a remote robot when the movement involves contact with objects and when the control 
involves a significant time delay. We also examine how object localization techniques can 
be used to overcome some of these difficulties. 

2.1 Time Delay and the Tele-Autonomous System Concept 


2.1.1 Time delay problem and predictor display 

As described in the previous chapter, the growing demand for applying tele-manipulation 
technology in space applications has resulted in considerable work in telerobotic research. 
While pure teleoperation techniques have been successfully used in undersea operations 
and nuclear industry for years, researchers have found them difficult to apply to the space 
environment due to transmission or telemetry time delays. The signal transmission delay 
affects the operator’s use of the remote manipulator. When direct telecontrol is employed, 
the operator’s strategy of performing a given task usually consists of a sequence of open- 
loop moves, e.g., each move is followed by a wait of one roundtrip delay time to check the 
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feedback signal when it arrives. In a space environment, when the control signals are from 
earth, the time delay can be quite significant, making it very difficult, if not impossible, 
for the operator to control the remote robot accurately and efficiently. 

The time-delay problem exists not only in tele-manipulation tasks where an operator 
controls a remote mechanical device or a telerobot to perform a manipulation task, but also 
in many other remote control tasks, such as the tasks of guiding and controlling certain 
kind of remote vehicles - spacecraft, aircraft, ships or cars. Researchers and scientists 
have approached this problem in many ways most of which are based on some forms of 

prediction with respect to the time-delayed signal. 

An early attempt to deal with the time-delay problem in tele-manipulation was made 
by Ferrell and Sheridan [40]. A controller was built at the local site, which had a structure 
similar to the remote manipulator arm. The controller was worn by the operator on his 
shoulder so that he could move it to maintain its orientation relative to the remote arm. 
The operator drove the controller with no time delays present, with the control signals 
being sent to the remote site as well. The local controller might be more properly called 
the predictive manipulator because the local controller was used to control the future 
movements of the remote manipulator. 

Bernotat and Widlok described the use of prediction display to guide and control 
remote vehicles [11]. In their work, the term “prediction display” was defined as 
“A display which gives information about the future status of a value. 

They found that predictive display of one or more system variables such as position, 
speed and acceleration in the form of points, curves, etc. was very useful in permitting 
the pilot to achieve smooth, stable control of the remote vehicles. Kelley used a similar 
approach in submarine operation and found substantial improvement in the performance 
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through prediction display [71]. 

Another use of predictive display was for the optimal control of launch vehicles during 
boost [44]. A manual guidance system which combines the use of predictive displays 
and optimal control theory enabled the operator to generate continually a predicted fuel- 

optimal trajectory. 

A significant progress on the time-delay problem has been achieved recently by Noyes 
and Sheridan through a method called “ predictor display ’ [89] or “forward simulation” 
[28]. In this scheme the operator directly drives a local simulator of the telerobot rather 
than the real telerobot with the control signals being sent, in parallel, to the simulator 
and the remote robot. The graphical display of the undelayed telerobot simulator is then 
overlaid onto the video pictures returning from the remote site. Obviously, the simulation 
at the local site operates in a predictive fashion, or in a forward simulation mode. That 
is, the simulation the operator controls is what the remote manipulator will be doing 
several seconds in the future. This method allows an operator to move the manipulator 
and immediately see the result of the action without waiting for the return video signals. 
Experiments have shown that if high quality models axe available, repositioning tasks can 
be done much faster than without using forward simulation [54] [28]. An example of the 
predictive display system concept is presented in Fig.2.1. The wire frame telerobot is the 
forward simulation of the remote robot, which directly responds to operator control, and 
the solid frame represents the time-delayed image of the real telerobot. 

From above description, we know that the “predictor display” is an extension of the 
predictive manipulator idea. In predictive manipulator control, the operator controls a 
local manipulator which has a structure similar to the remote manipulator; in “predictor 
display” the operator controls a graphical simulation of the real manipulator. Thus, the 




Figure 2.1. An example of prediction display. The human operator is 
controlling a simulated telerobot (dotted robot) while the 
position of the real telerobot is displayed in shaded color. 
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“predictor display” not only provides a method of dealing with the time-delay problem, 
but also take the full advantage of computer graphics, geometrical modeling, computer 
simulation and other related techniques to greatly increase the operator’s control ability. 
When discussing the “predictor display” method, we should note the difference between 
the concepts of “predictor display” and “prediction display”. While “predictor display” 
presents a simulation of the tele-manipulator (a 3-D object) on a graphical screen and the 
operator is able to control the movement of the simulator in real time, the ’’prediction 
display” only displays current and predicted values of variables in the form of curves or 

pointers. 

2.1.2 Time and position desynchronization 

Conway, Volz and Walker [28], [29], [30] have further developed the concept of predictor 
display by breaking the synchronization between the simulated time frame and that of the 
remote robot. They introduced time clutch and position clutch control modes to allow the 
operator to desynchronize the time and position frames, respectively, of the simulation 

and the remote robot. 

Time desynchronization control enables the operator to disengage the “direct-gearing 
of simulated-time and real-time and move the simulator as fast as skill and judgement 
will allow. Their hypothesis is based on the assumption that the human operator can 
often think of and generate a path segment more quickly than the telerobot can follow 
it. This assumption is particularly valid for large space telerobots such as the Remote 
Manipulator System [87]. As a result, such a generated path segment can be Mowed 
by the telerobot at nearly its maximum speed. Experiments have shown that when the 
time clutch is used to perform the task of touching a series of boxes, the performance is 
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better than the performance with no time clutch even in the case of no time delay [31]. 

In the position desynchronization mode the operator moves the simulated robot without 
sending any control signal to the remote site. This allows the operator to carry out difficult 
maneuvers on the simulator and allows the real robot to move directly to the end result 
(when the position clutch is released) without following all of the operator’s false moves. 
Thus, these control modes provide much greater control capability than previous methods. 

2.1.3 Basic tele-autonomous architecture 

The control modes mentioned above together with other control mechanisms such as the 
time brake, time ratio logic, task handoff and rendezvous constitute the basic frame of 
a tele-autonomous system. One such an experimental system has been implemented at 
University of Michigan. A detailed description of its implementation can be found in [30]. 

The basic system architecture is given in Fig.2.2. 

At the local control site a force-sensing joystick is used as an input device for the 
human operator to control the simulation of the telerobot. The input signal to the joystick 
is sampled and sent to an input transducer, where the positional signal is generated based 
on this sample. The input is then sent to a path history buffer (PHB) which is controlled by 
the PHB controller. A manipulator geometrical model, models of the remote environment 
and a path history animator (MGA) are built into the system in order to simulate the 
remote site. Other control mechanisms such as a time clutch, position clutch, time brake, 
etc . can also be found in the diagram. 

The system at the remote site consists of a remote manipulation controller (RMC) 
which can faithfully follow new position commands, a remote control queue (RCQ) that 
can hold command values sent from the local site and its associated queue controller (QC). 




Figure 2.2. Details of Basic Tele-autonomous System Architecture. 
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The detailed structure and functions are described in [30]. 

2.2 Imperfect Simulation Problems 

As can be seen above, predictor display and other control modes have been introduced 
to defeat time delay problems. The fundamental principle behind these methods is the 
direct control of a simulation of the telerobot rather than direct control of the actual 
robot. Because simulation is used, the quality of the model will be an important factor in 
determining the quality of results produced. One problem will arise: Can we build a high 
quality model? The building of a rather precise geometrical model for each individual 
component and even the remote environment is not totally impossible. In most cases, the 
telerobot and key components of a space station, shuttle, satellite, etc., are designed and 
built on earth. The environment in which the telerobot will work is also rather structured. 
All of these will allow us to build a quite good computer model. But in practice various 
uncertainties still exist and these uncertainties will result in imperfect simulation. These 
uncertainties include the following: 

. Imperfect simulation of the remote robot - The human operator moves the sim- 
ulated robot and assumes that the remote robot will reach the same position and 
orientation several seconds later. The remote robot, however, will in general reach 
a different position even though the error may be small. 

• Imperfect simulation of the remote environment — The object’s location is not 
exactly the same as anticipated even if an accurate object model is known. 

• Imperfect part model — Usually the part tolerances are not built into the model, 
which will result in geometric uncertainties. 
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What are the possible problems the imperfect simulation will cause during the imple- 
mentation of a tele-manipulation task? 

Consider now, in a more detailed way, what is involved in a tele- manipulation task. 
The process of implementing a tele-manipulation task, no matter how complex, involves 
repeated performance of the following sequence of activities. 

1. move the end-effecter of the tele-robot from the current position to a new one; 

2. grasp an object; 

3. move the object to another location; 

4. manipulate the object. 

We call steps 1 and 3 global motion activities and steps 2 and 4 fine motion activity. 
Table 2.1 gives a list of tele-manipulation tasks specified by RATS (Robotic Assessment 
Task Set). The operations of all the tasks follow this pattern. 

During global motion activities, one of the main concerns is collision avoidance. It 
is obvious that contact could be avoided in this stage. Also, as long as the collision 
avoidance requirement can be met, the path along which the robot’s end-effecter moves 
from the initial position to the goal position usually need not be very accurate. Therefore, 
the existence of imperfect simulation is not a crucial factor to the success of global motion. 
We anticipate that in the near future the implementation of global motions will still be 
under direct manual control. The state of the art is not yet good enough to do global 
motion planning automatically in real time. Yet, as has been demonstrated [29], tele- 
autonomous control is capable of doing all of the global motion tasks. During the fine 
motion stage (including grasping), however, contact is inevitable and will cause difficulties 
during the execution of the tele-manipulation tasks. 

Consider now what might happen in the following scenarios, where the remote ma- 
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• Assembly 

- Truss Assembly 

- Utility Line Connection 

- Station Interface Adapter (SIA) to Truss Connection 

- Solar Dynamic Array Assembly 

• Maintenance & Servicing 

- Solar Power Converter ORU Changeout 

- HRSO File Canister Changeout 

- SMM MEB Replacement 

- Electrical Connector Inspection 

- GRO Refueling 


Table 2.1. A Partial List of Robotic Assessment Test Set (RATS) 

nipulator’s movement involves contact with other objects. If the above described tele- 
autonomous system structure is used but only straightforward teleoperations are applied 
and the simulation is not quite perfect (as will always be the case), then the following 
scenarios occur: 

Scenario 7: More and P/ace 

A path that places an object on a surface in the simulation might actually 
extend into the surface in the corresponding real situation. Thus, when the 
real robot reaches the surface, it will continue to try to move through the 
surface, creating large forces unless there is compliance in the system. Large 
forces could either damage the parts or the robot or cause misalignments that 
would prevent success in further robot operations. 
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Scenario 2: Pep-in- Hole 

Let the remote robot be compbant so that large forces do not build up. Now, 
suppose that the operator successfully places a peg in a hole in the simulation 
and the real robot tries to follow this action. Because of various uncertainties, 
the part held in the hand of the real robot might hit the edge of the hole. While 
large forces will not build up, the operator does not know that the operation 
was not successfully completed, and the remote robot does not know what 
path to follow to successfully complete the motion. Indeed, the remote robot 
does not have a concept of what success would mean for the operation, except 
to have matched the commanded position, which it cannot do. 

In both cases, the imperfect simulation has resulted in the failure of the operation. 

In summary, the fundamental problem facing telemanipulation is the time-delay prob- 
lem. Predictor display and forward simulation provide one means to cope with this prob- 
lem. In this case the local operator is driving a simulator of the tele-manipulator instead of 
the real one, with the control signal being sent to the remote site in parallel. But imperfect 
simulation and uncertainty of the remote environment makes simple direct teleoperation 
difficult to apply in situations where the movement of the remote manipulator involves 

contact with objects. 

One solution is to develop a highly intelligent remote manipulator so the local operator 
needs only to give a high level instruction to the remote site and let the remote manipulator 
decide how to act. While a highly intelligent remote manipulator is what the people 
have hoped for and recent advances in artificial intelligence, human cognitive modeling, 
and computer vision indeed have provided us a greater opportunity than ever to build 
very powerful remote manipulation systems, they are still not powerful enough for fuU 

automation. 

As an alternative, human-guided control can offer an attractive possibility. That is. 
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the remote site has only limited intelligence and is still able to finish quite sophisticated 
tasks with the help from the human operator. As the technology evolves, the remote site 
will gradually take more responsibility until full automation can be achieved. This, in 
fact, is the core idea behind the tele-autonomous system concept. 

The uncertainty problem usually can be approached from two different objectives: 1) 
develop techniques that help avoid the uncertainty; and 2) techniques that help the system 
adapt to the problems. Several possible approaches have been suggested in [130], such 
as simultaneous force and compliance control [80] [95] [126] [128] [136], force/torque and 
contact constraint reflection [35] [83], dynamic replanning [7] [34] [131] [132] [133], etc., 
and there has been some success with these approaches. Their applications, however, are 
very limited. This is because either their methods can be only applied under very special 
conditions, or the experimental results axe not very persuasive [56] [74], 

In the next section we will describe how object localization technique can provide the 
necessary intelligence in the remote site to overcome some of the uncertainty problems. 

2.3 Applications of Object Localization 

The tele-autonomous system is designed to allow many different levels of control to be 
carried out in order to achieve maximum flexibility and efficiency. Although there is 
no general agreement on how to divide these levels and how to name them, one thing is 
certain: the classification of control modes is based on the degrees of operator involvement 
in telemanipulation, the ways in which the operator interacts with the remote system and 

the levels of intelligence in the remote system. 

In this section, we discuss two situations where a low level of intelligence introduced 

through object localization could help the execution of telemanipulation tasks. 
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Situation 1: Direct manual control is used to perform a telemanipulation task. That is, 
all the remote manipulator’s operations such as positioning, grasping and manipu- 
lating follow the local operator’s instructions. 

Situation 2: Limited autonomous control where the remote system has a certain degree 
of automation ability. The operator needs only to send high level commands to the 
remote site and lets the remote site perform a task (or subtask) automatically. 

The tele-manipulation tasks that fit into the first situation include the tasks which 
will be performed only “once in a life time” or performed infrequently. For these tasks, 
automation is very expensive; or very difficult to implement. Many operations such as 
infrequent maintenance operations or actions reacting to unforeseen events are examples 
of the first type of tasks. 

The tele-manipulation tasks that fit into the second situation include the tasks which 
will be performed repeatedly with no unforeseen events likely to happen during execution, 
or tasks which are very easy to automate even they may only be executed infrequently. 
Many Space Station Construction tasks such as “Truss Assembly” and “Solar Dynamic 
Array Assembly”, as listed in Table 2.1, are examples of the second category of tasks. 

2.3.1 Basic assumptions and the concept of relative move 

Our approach to solving the uncertainty problems under direct manual control is to com- 
bine the use of object localization’s technique in the remote site and the use of relative 
move mode of operation. This approach is based upon several reasonable assumptions: 

• The local simulation contains correct models of all the objects in the workspace and 
the locations of these objects is approximately known before any telemanipulation 
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can be applied on the object*. The approximate location of an object can either 
be modeled beforehand or be established through remote sensors. In the later case, 
we assume that TV pictures of the remote site can be viewed by the local operator. 
Whenever the operator finds an object from the TV pictures in which he/she is inter- 
ested and the location of which is not modeled in the local world modeling system, 
he/she can send a command to ask the remote sensors to provide this information 

to the local site. 

. Remote sensors can accurately locate objects in the workspace in renl-iime; they 
just cannot send the information to the local controller in a timely manner. 

. The objects to be manipulated and the simulations of the objects in the actual 
and simulated workspaces, respectively, are sufficiently close to each other that the 
differences will not lead to a singularity or violation of workspace problems. 

With these assumptions the basic idea of this approach is for the operator to control 
the simulated robot exactly as described earlier for .^autonomous operation. However, 
instead of sending the absolute position commands to the telerobot, the positions are 
first converted to being rulaiiue to the object being manipulated, and the relative position 
transmitted. The remote robot, by assumption, is capable of sensing the position of the 
object to be manipulated in real-time. It then uses the sensed position of the object 
to transform the received relative commands into absolute position commands and then 
proceeds to follow the commanded positions. If the system is sufficiently accurate, as 
described above, the commands should succeed. 
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• s[et] < obj >: Set reference frame to obj frame. 

By default, the reference frame is the world frame or robot base frame. The reference 

frame could be set into any other object frames which must have been modeled in 

the world modeling system. 

• m[ove] < pos >: Move the telerobot’s end-effecter to next position pos. 

The position has six parameters: three for translation, three for rotation. The 

position specification is relative to the current reference frame set by the latest s 

< obj > command. 

• l[ocate] < pos >: Locate object. 

Determine the location of the object set by the latest s < obj > command. In ordei 
to provide assistance to the remote system and speed up the localization process, 
the command has an optional parameter pos that tells the remote system which part 

of the object will be sensed by the remote sensors. 

• g[rasp]: Grasp the object. 

• r[un] < prog >: Begin to run a program named prog. 

Table 2.2. A list of the Basic Commands to Implement Relative Move 

2.3.2 Implementation of relative move 

To implement the new control mode, in addition to simply sending a sequence of servoing 
positions to the remote site, the local site needs also to send other necessary commands 
to the remote site to guide the operations of the remote manipulator. A list of the basic 
commands are described in Table 2.2. 

Let us take a closer look at how to use the relative mode and object localization 
technique to control the telemanipulator. For purposes of illustration, a simple move- 
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then-grasp operation is to be performed by using the direct continuous teleoperation 
control method. 

Assume that the operator will use a force-sensing joystick to directly control the move- 
ment of the simulation of the telemanipulator. The following symbols will be used to 
express position input and timing of the operations in the local site (see (31)): 

. T The simulation sample period, at each interval of which the input signal from 
the joystick will be sampled. 

• F(i): The input sampled at time i*T a . 

. p(i): The position of the tele-manipulator’s end-effecter calculated from the sample 

F(i). 

P(i) and F(i) are each six-dimentional vectors where F(i) is a vector of the force and 
torques measured at the joystick and P(i) is a vector of thre^translational and three- 
rotational coordinates. P(i) is calculated as: 

P(i) = P(i- 1) + AP(i), ( 2>1 ) 

where AP(i) is a function of F(()t) and other related variables. In a simplified case, AP(i) 
can be expressed as 

AP(i) = K(i) * F(i) ( 2 - 2 ) 

and K(i) is the gain parameter. 

By default, the world frame is initially set to the reference frame and the calculated 
position P(i) will be with respect to the world frame. If the reference frame is set to an 
object frame, as is needed to implement the relative move, the calculated P(i) has to be 
transformed into the position vector which specifies the position of the tele-manipulator's 
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end-effecter with respect to the object frame before its value can be stored into its ded- 
icated local output buffer. There is no problem for the transformation, because by as- 
sumption the relative location between the world frame and the simulated reference object 
frame must have been established in the world modeling system. The formula of the trans- 

formation is 

P(i ) 0 = n P(i) w ( 2 - 3 > 

where 

P(i) w is the position value of the simulated end-effecter calculated from the 
joystick’s reading with respect to the world frame; 

P(i) c is the transformed position value of the simulated end-effecter with re- 
spect to the reference object frame; 

T° w is the simulated transformation matrix between the world frame and the 
reference object frame. 

The position value P(i) 0 will then be put into the output buffer each time it is calculated. 

During each sample period T s , the P(i) value in the output buffer, no matter which 
reference frame it uses, is transmitted to the queue in the remote system, and then the 
new value of the position P(i + 1) is put into a local output buffer to be sent during the 

next sample circle. 

The control sequence in the local site is first to “move” the simulated telerobot close 
to the simulated object, then to “set” a new reference object frame, followed by sending a 
“locate" command; and then continuously “move” the simulated telerobot, still in absolute 
mode, with the position values being constantly transformed into the ones relative to the 
reference object frame and then executed; and finally grasp the object. 

If the operator feels that it is more convenient to graphically control the movement of 
the simulation of the telerobot relative to the reference object frame, he can do so without 
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any difficulty. The sampled data from the joystick can be set to represent position and 
orientation values relative to the reference frame instead absolute values (in this case, the 
transformation process of using Eq.(2.3) is no longer needed). The computer graphics 
techniques enable the operator to view the simulation with great flexibility in any angle, 
in any size, focus on any part of the environment, etc.. All these abilities make the relative 
move mode technically feasible. 

Now, let us look at how the remote system will execute the commands from the lo- 
cal site. These commands are held by a remote control queue (RCQ). Because in tele- 
autonomous system the time can be de-synchronized, there is no fixed relationship between 
the position of any entry in the queue and the time at which the entry will be processed 
by the remote manipulation controller, though the relative time ordering between entries 
will be preserved. 

When the reference frame is the world frame, the position value in the queue can be 
used directly to control the manipulator. If it is not, the position value which specifies the 
relative position between the end-effecter and the reference frame has to be transformed 
into the value which specifies the relative position between the end-effecter and the world 
frame. The task facing the RMC when it finds a locate command is to find the relative 
transformation between the referenced object frame and the world frame in real-time. 
Once the transformation relationship has been found, the remote controller will continue 
executing position commands in the RCQ. But the position value has to be transformed 
so that it is the value of the end-effecter with respect to world frame 

P(i) w = T“P(i) a (2-4) 

and the derived position value will be used by the RMC to move the telerobot. 

Table 2.3 shows the timing of the operations on both the local site and remote site. 
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Table 2.3. Timing of Move-Grasp Operations on Both Sites 

The key to the success of the relative move mode is the concept of time-desynchronization 
such that the timing in the remote site does not have to follow the timing in the local site. 
Thus, the position commands generated at a constant rate (1 command per T, unit) at 
the local site are performed by the remote controller at varying times: 7 i,T 2 ,- • • ,T J+n . 
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2.3.2. 1 Signal feedback - a method of improving the perfection of local 
model 

We have mentioned that imperfect simulation, especially imperfect simulation of object 
locations, will cause many problems during a tele-manipulation task and that object lo- 
calization can be used to cope with this problem. In this section we will show that the use 
of object localization can improve the results of imperfect simulation of object locations. 

Usually, only position values of the remote manipulator’s joints are sent back to update 
the local model each time immediately after the tele-manipulator is moved to a new 
position. Now, if after each “ locate ” operation, the position values of the referenced object 
frame are also sent back simultaneously with the telerobot’s position values to update the 
modeled location of that object frame, the updated model object location in the local site 
will become much more accurate than before, provided that the following conditions are 

met: 

l xhe sensing system in the remote site can provide accurate measurement, 

2. The telerobot position values provided by the RMC are quite accurate; 

3. The sensing system in the remote system is well calibrated such that the relative 

location between the telerobot and the sensor is known accurately. 

4. The model object has not been moved by the operator at the time when the updated 

object location values are sent back; 

Let us assume that the sensor is mounted somewhere on the telerobot’s gripper. Let T a b 
represent a transformation matrix between coordinate frame a and coordinate frame 6, 
symbols g,o,s represent, respectively, the gripper frame, the referenced object frame and 
the sensing frame in the remote site, and mg,mo,w represent, respectively, the modeled 
gripper frame, modeled referenced object frame and the world frame in the local model. 
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After a “ locate ” operation, the sensing system can provide an accurate measurement 
of T* (condition 1). From condition 3, a transformation matrix T 9 will also be available 
accurately from the formula: 

Tf = T 9 3 T 3 0 (2-5) 

The values of the transformation matrix together with the position values of the telerobot s 
gripper will be sent back to the local site simultaneously. The same values will be treated 
as the position values of corresponding modeled coordinate frames, e.g., the position values 
of gripper frame will be thought as of T” g and T 9 wiU be treated as T™ 9 . Condition 2 
guarantees the accuracy of matrix Therefore, the transformation matrix 

rpw _ rpw rpmg ( 2 . 6 ) 

± mo rng M mo v ’ 

will become quite reliable and can be used to update the modeled location of the reference 
object. The time-delay has no influence on the result of updating because the object s 
position is fixed during this period of time from condition 4. 

Once the object location model has been updated with high accuracy, the operator 
will have greater confidence in controlling the simulation of the telemanipulator. If the 
modeled object location is not very distant from the true location, the operator might 
even feel that nothing has been happened during his continuous tele-manipulation. 

2.3.3 Object localization used in autonomous operations 

The Object localization technique can also be used in situations where the remote 
system operates in autonomous mode. The operations usually go through the following 
sequence: The operator moves the simulated tele- manipulator to a pre- specified position 
and then sends a command to the remote system to activate a pre-programmed routine 
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Figure 2.3. A Simple Routine With Relative Position Specifications 

for the tele-manipulator to execute. The routine is coded in such a way that the position 
path is specified relative to a reference object frame instead of the absolute value. Before 
the execution of the routine, the remote system has to first “ locate ” the reference object 
and then convert the specified positions path to the position path of the tele- manipulator s 
end-effecter. 

For example, if a task of cleaning the surface of a Solar Dynamic Array Plate (SDAP) 
is to be performed by the tele-manipulator, the cleaning tool which is attached on the 
tele-manipulator’s end-effecter will move along the surface of SDAP following a certain 
path. The path of the cleaning tool is specified in a routine by a series of positions, all 
are specified with respect to the SDAP coordinate frame - another form of relative move 
(see Figure 2.3). 

Each time, before the telerobot executes the routine, it first locates the SDAP. The 
real path of the end-effecter with respect to the world frame can then be calculated by 
using the formula shown in Eq.2.4. Thus, the routine outlined above gives the telerobot 
the flexibility to adapt to various locations of the SDAP. 




CHAPTER 3 


METHODS AND STRATEGIES OF OBJECT LOCALIZATION 

In the last chapter we have discussed the importance of object localization techniques 
in tele-autonomous systems applications. Indeed, the availability of efficient means of 
locating objects is one of the key factors to the success of developing such systems. In 
addition, object localization techniques have also found many applications in other areas of 
technology, specially in robotic manufacturing such as automatic assembly, part inspection 

and so on. 

Object localization has long been defined as a part of the object recognition process in 
computer vision research [12]. But in most instances the emphasis of the research has been 
on object recognition. Object localization is only a by-product. In robotic applications, 
however, object localization usually is the ultimate goal. It has many of its own problems 
to be solved, such as real-time considerations, accuracy issues, types of beatable objects, 
working conditions, etc., which object recognition research generally does not address. 
In some telerobot systems “locate” has been defined as one of the basic independent 
operations the system is to perform [96]. As a result, the object localization problem, as 
an independent research topic, has attracted increasing attention recently. 

This chapter will give an overview of the three-dimensional object localization problem. 
First, it provides a closer examination of the problem and then describes general object 
localization system structure. This is followed by a discussion of some important issues 
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of object localization. Current available object localization algorithms and systems are 
surveyed in the last section. These algorithms are characterized by their feature and 
matching strategies, the range-Bnding methods, the types of locatable objects and the 

mathematical formulating methods. 

3.1 The Object Localization Problem 

Object localization is the determination of the location of stationary and moving ob- 
jects. What must be solved when a robot vision system is trying to locate an object . s 

Gunnarsson pointed out [49]. 

Localizing a part means being able to answer the following question: “ In a 
given frame of reference, what are the Cartesian coordinates of any spec.fie 

point on the part’s surface. 

to practice, it is both impossible and unnecessary to take real measurements on every 
point of the object’s surface in order to locate it. Instead, in a model-based system where 
the shapes of the objects in the scene are known and a complete geometric database 
description of each object with respect to that object’s coordinate frame is available, 
the localization problem is solved by identifying the location relationship between two 
coordinate frames: the reference frame and the object coordinate frame. Once the location 
relationship between these two coordinate frames is determined, the Cartesian coordinates 
of any point on the object surface with respect to the reference frame can be obtained by 

a simple transformation process. 

To locate an unknown object, the best way is to first explore the object and try to 
establish a model description for that object before any localization process is carried out. 
Recently, researchers have begun working on this object exploration problem and have 
achieved some preliminary results [33] [104]. 
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Thus, a. necessary component of every object localization system is the world, modeling 
system which stores, among other things, the descriptions of all the object geometrical 
shapes and a definition of the sensing coordinate system. Each object must have a cor- 
responding coordinate frame associated with it in order for the object to be described in 
the world modeling system; the coordinate frame might be specified either implicitly or 
explicitly. 

The relative location between two coordinate frames can be specified in any one of the 
following ways: 

1. Position and orientation: 

The position is usually specified by a 3 x 1 position vector p = ( p x ,P y ,Pz )• There 
are three different representations of orientation: 

• Represented by three rotation angles: this could be Eular angles or the 

rotation angles about the coordinate axes. 

• Represented by a unit vector r and an angle 0. 

• Represented by a quaternion or its variation [64] [70] [94] . 

In practice, the terms of “rotation” and translation are also frequently used to 
represent the relationship between two coordinate systems. They have the same 
meanings as “position” and “orientation”. Both terms are used in our discussion. 

2. A 4 X 4 homogeneous transformation matrix. 

3. Dual number quaternion: [121] 

This is an extension of quaternion representation in which each quantity is changed 
to a dual quantity [26]. The dual quaternion has a similar interpretation as the real 
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quaternion: 


9 = 


sin(0/2)n 


[ cos(^/2) J 

where the vector n is a unit line vector about which the coordinate system has been 
rotated and translated and 6 is the dual angle of rotation and translation. 


The objective of the object localization algorithm is to determine the parameters which 
specify the corresponding representation. 

Which representation method is the best one to choose in real application? It depends 
on many factors. But among other things, how this localization problem is formulated in 
mathematical terms and which mathematical tool is used to carry out the computation 

are definitely the main factors in the selection. 

For instance, if geometrical analysis is used to derive the location relationship between 
two coordinate frames, vector algebra is the right tool during the derivation. The relative 
location between these two frames is best expressed by the vector representation method 
- a position vector and a rotation direction vector together with a rotation angle. In 
many cases, the localization problem can be formulated as an optimization problem using 
a least-squares minimization to solve the problem,. The quaternion representation is a 
better choice here because it does not involve any trigonometric computation and the 
number of unknowns to be optimized are nearly the minimum. 

In some applications the localization problem can be simplified due to extra constraints 
imposed on the object. For example, if an object is so constrained that a planar surface 
of the object is always lying on another planar surface, there are only three degrees of 
freedom for the object: one degree of rotation and two degrees of translation. 

How does one solve for these position/orientation parameters if sensor data and object 
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models are given? Usually the computation is carried out by a matching process. That is, 
the object localization algorithm will try to find a “best” transformation which will put 
sensed features into its corresponding model features. The matching feature pairs may 
be of the same type or may be from dilferent types; they may be 2-D features or 3-D 
feature. But all the features are of geometric types, e.g., those that specify position and 
orientation, such as points, edges, surfaces and etc.. Table 3.1 shows some known feature 
matchings which have been used in the literature to derive the location of an object. 
Sometimes, a combination of feature matchings is necessary to completely specify a rigid 
transformation. The geometric features to be extracted and matched can be classified 
as low-level features and high-level features. Possible low-level features include points, 
vectors, fine segments, axes, surface patches, edges, boundaries, etc.. Possible high-level 
features include straight dihedrals, circular dihedrals [18], principle directions of surface 
curves, minimum, maximum and mean curvatures of surfaces, Gaussian curvatures, etc.. 
Usually the lower the level of features, the greater the number of features to be extracted. 

From the above description, it is not difficult to imagine that a general object local- 
ization system should contain the following components: (1) sensing system, to provide 
necessary measurements; (2) world model: to give a geometrical description of all the 
objects in the environment including robot, sensors and their relationships; (3) feature 
extraction: to retrieve geometrical features which are to be used in the matching process; 

(4) matching: to try to pair the sensed features with corresponding model features; and 

(5) computing: to calculate the transformation parameters. 

A general configuration is shown in Fig. 3.1. However, many variations could exist in 

real applications. 

Take the sensing system as an example. The task of the sensing system is to provide 
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Measured features 

Matched to 

Point 

Point 

Planar surface 
Surface patch 

Surface normal 

Surface normal 

Line segment 

Line segment 
Planar surface 
Surface patch 

Edge 

Edge 

Planar surface 

Planar surface 

Quadric surface 

Quadric surface 

Gaussian curvature 

Gaussian curvature 


Table 3.1. Known Matching Strategies in Object Localization 

enough data for the feature extraction unit. If a single measurement taken by the sensing 
system can provide enough sensed data, the connection between these two units is a one 
way relation. Sometimes, the sensing system has to make a series of measurements in 
order to meet the needs of the feature extraction unit, such as the case where a spot-range 
sensor is used. In this situation, the whole feature extraction might go through a repeated 
sensing-extraction process, and a scheduling algorithm may be necessary to guide the 
sensing-extraction process. The function of the scheduling algorithm is to find an optimal 
measurement path in order to reduce the measurement times. [102], [103] have described 

such an algorithm. 

Another example is the matching unit. The matching process is the process of finding 
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Figure 3.1. Object localization system organization 

the pairings of sensed features and model features. Depending on the intelligent level of 
the system, the matching could be performed differently. On the lowest level, the system 
has no matching ability at all. That is, there is no matching unit in the system. Such 
systems can often find applications in highly-structured environment in which the relative 
locations of all the objects are approximately known a prior. Thus, the system knows 
where to make necessary measurements in order to locate the objects, what features are 
expected to be extracted from the measurements and what matching modeled features 
correspond to the sensed feature. No matching process is needed in this situation. The 
matching process could also be provided manually. On telerobotics systems, for example, 
the teleoperator might interactively assist the model matching by indicating with a light 
pen which features in the image (e.g. edges, corners) correspond to those in a stored model 
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[3]. If none of these conditions are met, the system has to have its own matching unit to 
pair the features automatically. This requires systems with higher levels of intelligence. 

There is no common solution for the implementation of the object localization mech- 
anism we have just shown. System structure may have different configurations. Each 
component in the system could be implemented in various ways, too. 

3.2 Some Issues 


We have just shown that a general structure for object localization systems. In practice, 
there are some important issues which must be considered when a real localization system 
is to be designed. Among them, the important ones are the speed and accuracy require- 
ments of localization, the types of locatable objects, sensing methods, sensor installation, 

etc.. 

3.2.1 Real-time execution 

A Hierarchical control structure has been defined as a standard telerobot control archi- 
tecture [3] and has been adopted by researchers to develop individual telerobot systems, 
such as systems developed at Goddard [96], the University of Michigan [120], etc.. As 
the functions of vision systems are different at each level, so are the requirements for the 
object localization algorithms. Usually the higher the level, the slower the completion 
rate. See Table 3.2 for typical completion rates at each level of telerobot control. 

At the object task planning level, for example, one of the functions of the vision system 
is to recognize the environment. The object localization system, as a part of the vision 
system, is used to give approximate measurements of the locations of the objects in the 
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Average rate of 

Average 

Planning 


change in output 

replanning interval 

horizon 

Servo 

1 KHz 

1 millisec. 

15 msec. 

Primitive 

62Hz 

16 millisec. 

300 msec. 

El- Move 

8Hz 

128 millisec. 

2 sec. 

Object /task 

1 Hz 

1 second 

30 sec. 

Service Bay 

.1 Hz 

10 second 

> 10 min. 

Mission 

0.01 Hz 

1.7 minutes 

> 1 hour 


Table 3.2. The rate of subtask completion at each level of hierarchy. 

([ 3 ]) 

environment. The execution time is in the minute range. At the E-move level, however, 
the rate of completion is in the range of seconds. If a visual-feedback control strategy is 
used here, the localization system has to generate updated measurements for the control 
system to adjust the robot’s movement in the same time frame. Real-time issues will 
become important. Based on different timing requirements, the strategies of localization 
might be also different. 

3.2.2 Accuracy 

Accuracy is another important issue in object localization. There are two concepts about 
accuracy, e.g., absolute accuracy e and relative accuracy Ac. 

• Absolute accuracy is defined as the difference between a measured value m and the 
“true” value 5. That is, e = m — s. 
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• Relative accuracy is defined as the difference between a measured difference A p and 
it’s actual difference A p. That is Ac = Ap - A p. For example, if a point is moved 
Ax units along x axis and we measure the change of the movement as Am, the 
relative accuracy of the measurement is Ac = Am - Ax. 

Different types of telerobotics tasks require different accuracy. High accuracy requirement 
occurs primarily in assembly types of tasks. According to our survey of eight telerobotics 
tasks [96], for move-pickup-put types of tasks the accuracy requirement is 0.1 - 1 inches 
(2.54 - 25.4mm) in translation and 2-10 degrees in rotation; for assembly types of 
tasks, especially for insertion, the accuracy is much higher: 0.03 - 0.0625 inches (0.762 - 
1.587mm) in translation and about 1 degree in rotation. 

Another study on the accuracy issue of sensor-driven robotic assembly tasks is done 
by [47]. According to the author’s analysis on a typical peg-in-hole assembly task 
the most frequent assembly operation [72, 88], if a 1.75 inches (44.5mm) square xlO 
inches (254mm) length oblong peg is to be inserted in a hole with a clearance of 0.004 
inches (0.1mm) on each side, and we do not want the insertion to fail because of various 
misalignments (translational, rotational etc.) on the position between the peg and hole, 
the maximum allowable misalignments are 0.02 inches (0.5mm) in translation and 0.5 
degree in rotation by using [127]’s formula. These two figures are quite consistent. 

[47] has further analyzed the accuracy requirement for the sensor. He concludes that if 
a 98.8 percent chance of successful assembly is assumed and the errors from other resources 
such as robot positioning error, robot kinematics error, sensor- robot coordinate alignment 
error etc. are also considered, the sensor should provide localization accuracy up to 0.0055 
inches (0.14mm) in translation and 0.14 degree in rotation. 

The accuracy requirement in an assembly task is in fact based on the relative accuracy 
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most of the time, not the absolute accuracy. This is because during an assembly the 
system only needs to know the relative location between the two relevant objects. 

Absolute accuracy to a large extent depends on the accuracy of the sensing system. 
The achievement of high relative accuracy, on the other hand, does not necessarily mean 
the use of highly accurate sensors. The achievement of high relative accuracy usually 
requires a good design for the object localization algorithm. 

Therefore, when designing an algorithm, one must evaluate its performance according 
to both it’s absolute accuracy and relative accuracy and the emphasis should be on the 
relative accuracy. This is because: 1) the tasks which require high accuracy are that of 
assembly types in most cases, and 2) performing an assembly task mainly requires relative 
accuracy. 

3.2.3 The types of locatable objects 

It is best if the system can locate arbitrary-shaped objects, as long as the shapes are 
describable. For an arbitrary-shaped surface, the surface curvature properties such as 
Gaussian curvature, minimum, maximum and mean curvatures are often used as the 
features to match in order to locate the object. The problem of using these features is 
the high-sensitivity associated with either measurement errors or slight distortions of the 
object surfaces [49]. 

In industrial environments a more practical requirement for the sensing system is the 
location of man-made objects. The shapes of most of these industrial parts are not very 
complex. Their surfaces contain only one or a combination of a few simple and well-known 
types of surfaces, e.g., planar surfaces and quadric surfaces (cylinders, cones, spheres, 
etc.) [14]. If a localization system is able to locate objects having such types of surfaces 
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reliably, it will work for most of the time. How to deal with complicated objects? Instead 
of developing a universal localization algorithm, a simple alternative solution is to make 
special marks on the object so that the localisation system has no problem detecting these 
marks and uses these detected marks as the matching features to compute the localization 

parameters of that object. 

Therefore, among other things, localizability should also be a consideration during 
the design stage of an industrial part. Some guidelines should be given to meet the 
localizability requirement. Sometimes, very simple modifications made on the part design 
can greatly ease the part-localization process. 

3.2.4 Sensing system 

What types of sensing techniques should be used in a localization system? Where should 
the sensors be installed? How should the basic sensor requirements be determined in 
specific applications? These are just some of the issues in the design of a sensing system. 

3.2.4.1 Ranging methods 

Jarvis (631 has presented an early overview of range finding techniques. The range-finding 
methods can be classified, based on the types of illumination, into two categories: passive 

methods and active methods. 

Passive methods use normal, unstructured or natural illumination to acquire simple 
images from 2-D cameras, then process these images to obtain distance information. Oc- 
clusion cues [97], texture gradients [125], shape from shading [93], depth from focusing 
[58] [62], stereo disparity [24] [57] [61] [129], range from motion [119] [123], moire fringe 
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contours[59] are all examples of passive techniques. Because range information is extracted 
from intensity images, it requires extensive effort to process images and is thus difficult to 
be used to solve the real-time object localization problem. 

Active methods use controlled energy beams, such as ultrasonic, radio, white light, or 
laser beams, to acquire range information through the detection of the reflected energy. 
Because of the obvious advantages of laser beams over other energy sources, most sensors 
use lasers as the energy source. Based on their range finding principles, the active range 
measuring techniques can be divided into the time-of-flight method, triangulation method 
and striped lighting method. 

The principle of the time-of-flight method is very simple. The distance of an object 
is determined by measuring the time it takes for the controlled laser beam to travel from 
the source to the object and back. In time-of-flight methods, the lasers can be used 
as pulsed-mode or modulated continuous-wave mode. In the pulsed-mode method, the 
time is measured by counting the number of pulses a laser beam takes to go from the 
source, bounce off a target point and return coaxially to a detector. To achieve high 
resolution, the electronic circuit must have fast response and high time resolution in order 
to detect and process returning signals. Modulated continuous-wave laser range-finders 
determine distance by measuring the phase between the received wave and a reference 
signal. For this type of laser range sensor, the depth resolution will depend on the waveform 
frequencies used by the sensors. The higher the frequency, the better the depth resolution 
will be. A range sensor built at the Toshiba Corporation, Heavy Apparatus Engineering 
Laboratory, using the phase-shift measurement technique has been reported [82]. It has a 
field of view of 28 x 28 degrees and has a dynamic measurement range from 0.2m - 1.4m 
(7.87 - bh.\2inches). By using a laser beam modulated at dual frequency of 1 GHz, the 
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Figure 3.2. Triangulation-based laser range sensor. 

sensor can achieve depth resolution of 0.075mm (0.00295tnches). 

Triangulation-based laser range-finders use geometric principles to obtain range infor- 
mation. The sensors project a beam of light with a known shape (point, line) onto the 
object to be measured. The reflected light is picked up by detector array. Trigonometry 
is used to compute the distance of the projected spot (or line) from the sensor head. No 
image analysis is required during range acquisition (see Fig. 3.2). This type of sensor is 
mostly suitable for short range measurement. This is because longer measurement distance 
requires a larger baseline distance between the laser source and the detector array, and, 
as a result, a larger sensor . The advantages of this type of sensor are fast response time 
and accurate measurement. The disadvantage is the unavoidable “missing part” problem 
caused by occlusion. The sensors can be made in different shapes. A spot range sensor can 
only measure the distance of one point at a time; line range sensor can measure distance 
along a line on the surface of an object; a multi-spot sensor can measure many points in 
a special pattern at a time (see Fig. 3.3) [66]. 
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Figure 3.3. Configuration of the Multi-Spot Source Proximity Sensor 

[ 66 ]. 

The striped lighting method is perhaps the most frequently used range finding method 
for locating object distance. In its simplest form, a single slit of light is projected onto 
the scene and the scene is viewed by an offset camera. If the light strikes an object, the 
reflected light will be imaged on the camera’s two-dimensional image plane. A one-to-one 
correspondence exists between all the points shown on the image and the points lying 
along the slit of light. The 3-D coordinates of each point which the slit of light projects on 
the object surfaces can be obtained from the corresponding 2-D coordinates in the image 
plane by optical triangulation computation. The striped lighting method in fact also 
belongs to the triangulation range sensing category. But it needs to process the image in 
order to extract the reflected light features. Because an image reflected from the surfaces 
of an object by a slit of light consists of straight lines and curves, it takes much less effort 
to extract these line or curve features than the effort in processing a general range image. 
The latter usually has to go through several processing steps such as segmentation, feature 
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Figure 3.4. A striped light range sensor 


grouping and etc., before the needed features can be extracted. Fig. 3.4 shows an example 
of this kind of sensor and its characteristics developed by Toyota Central Research and 
Development Labs [91]. 

Based on the same principle, many advanced striped lighting laser range sensing sys- 
tems have been developed recently, which exhibit greatly improved speed, accuracy, size 
and other sensor performance measures. [92] has reported a cross-slit lighting sensor. It 
is very compact: 140mm x 110mm x 47mm (5.51 X 4.33 X 1.85inches) in size and 800p in 
weight. It has a standoff distance of 100mm(Z) (3.93 inches) with 70mm(A') x 70mm(T ) x 
50mm( Z) (2.755 X 2.755 X IMSinches ) of measuring area and 0.25mm (0.0098mc/ies) of 
accuracy. It can measure 242 points on each slit and 6 frames per second. 

To speed up the ranging process, multiple beams of light can be projected onto the 
area of a scene. The key to the success in using the multi-slit method is the determination 
of the matching between a detected stripe and its original position in the projection grid. 
Once the mapping is established, the range of each projected light stripe imaged by the 
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camera can be calculated by triangulation. The matching problem can be solved by either 
sequential scan projection [90], or simultaneous projection with special pattern [124] such 
as color-encoded projection [22], random dotted pattern projection [79], gray-code pattern 
projection [100] [101] and so on. Because each slit of light can be matched, the slits can 
be processed independently, which leads to the potential of parallel processing and thus 

real-time sensing. 

3.2.4. 2 Sensor Installation 

Range sensor installation is also an important issue. The task of a sensing system is 
to provide object location information timely and accurately during the execution of a 
telerobotics task. For accuracy consideration, the sensors should be placed close to the 
object, such as on the robot’s end-effecter. Here the problem is how to determine the 
exact position between the object and the gripper after the gripper picked up that object. 
There are several possible solutions: 

1. Have a special fixture attached on the telerobot’s gripper so that whenever the 
gripper picks up an object, the object will always be “locked” in a pre-determined 
position. This method is feasible from a technology point of view, but not very 
flexible in practice. 

2. H the telerobot has two arms, each end-effecter can install one range sensor. The 
object’s position with respect to one gripper after the object is grasped by that 
gripper can be accurately measured by the range sensor on another arm. If needed, 
that arm can move toward this arm closely enough to make accurate measurement. 

3. In addition to the range sensor placed at the robot’s end-effecter, a global range 
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Figure 3.5. Rockwell Telepresence kit. [23] 


sensor can be installed at a global position, like the position of human eyes as we 


have seen in the design of some space telerobots (see Fig. 3.5). 


3.3 Feature Extraction & Matching Strategies 

As we have said, the object localization process basically is a feature matching process, 
that is. finding a best estimate of transformation parameters which will align some modeled 
object features with certain (perhaps different types of) measured object features. Based 
on how feature matching is realized, the object localization algorithms can be broadly 
divided into two categories: the algorithms which do not involve any recognition process, 
and those which have more or less a recognition process involved. We call these two 
types of algorithms direct-localization algorithms and recognition-localization algorithms, 
respectively. 

Obviously, the second type of algorithm has a higher intelligence level than the first. 
Even within the second type of algorithms, the intelligence levels can be different. Some 
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of them can establish matches within one object, some of them can do it within a group 
of the same type of objects, others can match the features within a group of different 
types of objects. At the highest level, the algorithm could locate unmodeled objects. To 
do this, a set of primitive features should be specified in a database, which will form the 
basic frames of any object to be constructed. Before localizing the unknown object, the 
algorithm must explore the object and establish a model for the object using the set of 
primitives. 

Each type of algorithm can be further classified according to its sensing methods, 
the types of features used for matching, mathematical formulation methods, the types of 
locatable objects and so on. 

3.3.1 Direct-Localization 

Direct-localization algorithms are primarily used in the situations where either the 
working environment is highly-structured, the position relationships among the objects 
in the environment have previously been established approximately, or human beings can 
provide assistance in defining the required measurements. The telerobotics applications 
in most space programs meet these requirements. 

Because no recognition is involved, the localization process is quite simple. The ex- 
tracted features and model features can be used as inputs for direct computation. The 
time of localization depends on the time spent on measurements and feature extraction 

computation. 

One method proposed by Gunnarsson and Prinz [49, 50] is based on their observation 
that if a set of points are measured and these measurements are distributed on the object 
surfaces, the best transformation is the one which will make the sum of distances between 
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each measured point and it’s corresponding transformed surface minimal. Their idea 
leads to a point-to-surface matching strategy. Their algorithm, when formulated in 
mathematical terms, becomes a least squares minimization problem and can be used 
to locate arbitrarily-shaped objects. Usually an iterative numerical procedure is needed 
to solve for the problem. The numerical procedure they used is a modified Lagrange 
multiplier and Newton- Raphson method. Because a good initial guess can be provided due 
to the fact that the object’s approximate location is assumed to be known, the convergence 
of the algorithm is guaranteed in most cases. 

Gordon and Seering [48] developed a system which uses striped-light and camera sens- 
ing to gather necessary range data. The system can only locate planar objects, line-to- 
SURFACE matching is used in their algorithm. The striped-light, when projected on the 
planar surfaces of the object, generates straight-line segments. The scene is then viewed 
by a camera. The equation of each line segment can be obtained by analyzing the cor- 
responding image of that line segment as viewed by the camera. Three independent line 
segments are needed to compute the rotation and translation parameters. The fact that 
the line vector is perpendicular to the rotated model surface normal vector can be used 
to derive the rotation. The algorithm uses quaternions to represent rotation and uses a 
numerical method to compute it. They also give a closed-form solution for the rotation 
when three mutually perpendicular surfaces are sensed. The calculated rotation is then 
used to compute the translation. 

The same striped-light and camera sensing system is also used by Rutkowski, Benton 
et al. [10, 98]. But their matching strategy is point-to-surface matching. In their 
algorithm, the measured points are from extracted line-segments, either straight or curved. 
Their method imposes no particular constraints on the shapes of the object surfaces, as 
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long as the object surfaces can be partitioned into a collection of primitive surfaces, such 
as planes, cylinders, or spheres. The computation is carried out by a repeated location 
adjustment. The location adjustment is expressed by three quantities: the rotation center, 
rotation axis and translation vector. To guarantee a fast convergence of their algorithm, 
the center of mass data point is chosen as the rotation center instead of the origin of the 
model’s coordinate system. 

When comparing these methods, we find that all of them have very high measurement 
accuracy and fast execution speed. For example, Gordon’s system can locate an object 
in about 2.5 seconds, including the times spent on image acquisition, image processing 
and computation, and can achieve a relative accuracy of 0.002 inches in translation and 
0.1 degrees in rotation when a two-inch cube is being located. It is capable of reliably 
assembling components with little clearance without using force controlled motion. In 
Gunnarsson’s algorithm the measurement error is the same order of magnitude as the 
sensor error. These algorithms also have some problems. The problem associated with 
stripped-light sensing is that it requires an extra light source with a special pattern, which 
sometimes is inconvenient. The use of a spot sensor or line sensor has the problem of 
multi-measurement, e.g., the sensor has to be installed on the robot’s moving part and be 
moved together with the robot in order to take multi-measurement. This will slow down 
the localization process. 

High-level features can also be used to locate objects. For example, Thorne and ct 
al. [115] describe an algorithm which uses features such as the radii or curvatures of a 
space curve along the surface to locate an object. The curvatures k or radii p of a space 
curve can be expressed as a function of the length s of the curve, e.g., k — k(s) (or 
p - p(s)), which is independent of the coordinates of the curve and is thus invariant under 
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rotation and translation. The algorithm assumes that there exists a particular feature 
line or fingerprint for each object. The feature line could be a certain portion of the 
curved edge(s) of the object or a curve on the object surface. A curvature plot along the 
feature line can be drawn. In the database, the feature line is specified by a set of discrete 
points. Associated with each point is information about its coordinate (x,y,z), radius of 
curvature, curvature, delta length, and total length. The total length is zero for the first 
point. The localization is proceeded through point-to-point matching. The method 
first measures a set of discrete points along the feature line and then finds a corresponding 
point for each measured point. A least squares optimization algorithm is used to find the 
location paxameters. A similar algorithm which uses Iso-Gaussian (a curve connecting 
points of constant Gaussian curvature) matching to localize an object has been described 
by Gunnaxsson [49]. 

3.3.2 Recognition- Localization 


In many applications the objects can be placed anywhere in the environment. There- 
fore, if a measurement is made and some sensed features are extracted, the localization 
system has no prior knowledge about which object or which part of the object the sensed 
features belong to. In this case, in order to compute the location of an object, a recogni- 
tion process is needed, which will establish the matches between a set of sensed features 
and the model features. 

There axe two popular matching strategies: tree searching and clustering. 

In the tree searching strategy, if there are k sensed features Si,i = l - k and l m model 
features Mj,j = 1 • • • / m for object 0 m ,m = 1 •••in, a searching tree can be constructed 
for each known object 0 m such that the tree has l m levels, and each intermediate node 
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has k branches. Each path from root to leaf represents a potential matching. The total 
number of possible matchings, or the searching space for object 0 m is which is very 
huge. To reduce the searching space, several methods have been proposed. 

One algorithm proposed by Grimson, Lozano-Perez et al. [ 43 ] [ 45 ] is to use local 
geometric constraints such as distance constraint, angle constraint, direction constraint, 
triple-product constraint and so on to reduce the searching space. Beginning from the 
root of the tree down, at each node, a local constraint test is made to see if the sensed 
features up to that level are consistent with these constraints. If not, the entire subtree is 
discarded from consideration. 

A similar tree searching method is used in Faugeras and Hebert’s work [ 37 , 38 , 39 ]. 
Instead of local constraints, rigidity is used as the basic constraint during the tree search 
process. Every path from the root to an intermediate node (level k for instance) represents 
a partial matching. The algorithm computes a best rigid transformation T fc up to that 
level ( k ). Then Tjt is applied to the next unmatched model primitive Mfc+i and only those 
sensed primitives that are sufficiently close to TfcMfc+i are considered. The computations 
are carried out by least squares optimization techniques. As each new pair of primitives 
adds to the partial matching list, the new estimation of transformation has to be started 
over again. The algorithm’s underlying paradigm is “ locating while recognizing ” which is 
different from the paradigm of “ locating after recognizing" used in the Grimson et. al. 

algorithm. 

Reducing the number of sensed and model features is another important method to 
speed up the tree searching process. The use of higher level features can effectively reduce 
the size of the searching tree because fewer features are usually adequate. The system 
developed by Bolles, Iloraud et. al. [18, 19] is such an example. Three different types of 
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edges are used as the primitive features. They are: straight dihedrals, circular dihedrals, 
and straight tangentials. They are higher level features: one pair of matched features can 
determine all but one of the object’s six degrees of freedom. 

Clustering is another technique used in recognition-localization algorithms. The prin- 
ciple of clustering is very simple: 

For each element in the sensed feature list 
for each element in the model feature list 

if they are compatible, compute a transformation candidate 
put it into cluster space. 

The cells with the largest counts are expected to represent the location. 

While the principle is simple, the implementation is not so easy. The high dimensions 
(six) and huge space of clustering are just two difficulties. Different methods have been 
proposed to accommodate these problems. Three dimensional clustering, the use of proper 
size of cells and hierarchical clustering are some of them [6]. Several systems have been 
proposed using the clustering technique. Linnainmaa et. al. [76, 77, 78], Silberberg, 
Harwood, et. al. [110], and Stockman et. al. [112, 113, 114] are typical examples. One 
property of clustering is the algorithm’s parallel structure, which will have an important 
impact on the future development of object localization algorithms. 

In most algorithms, the least squares optimization is the mathematical tool to estimate 
the best transformation if many feature-pairs are found. During the optimization process, 
bad data can influence the accuracy of computation. Therefore, a filtering process usually 
is needed to detect and remote bad data points. Another mathematical tool is based on the 
use of probability theory. Bolle and Cooper [13] [15, 16, 17] have presented a statistics ap- 
proach of combining pieces of information to estimate 3-D complex-object position. They 
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formulate the optimal object localization as a Bayesian probability estimation problem. 
The objective is to find the most likely transformation T that maps the model primitives 
onto the measured range data. The likelihood p(Y\T) should be maximized with respect 
to T, where Y is the measurement data. If n primitives have been extracted from range 
data and matched to model primitives, then p(Y|T) = FliUi P( y *l T )- That means that 
to arrive at a global optimal solution, the maximum likelihood estimation has to be ap- 
plied locally. Based on this analysis, they derived a different formula for minimizing the 
estimation error from the traditional least squares optimization formula. To arrive at an 
optimal solution, a thorough analysis of measurement errors and a good error model are 

needed. 

3.4 Summary of the Chapter 

We have discussed general object localization problems and localization strategies. Dif- 
ferent levels of telerobot control have different requirements on the localization system, 
such as speed, accuracy, the level of intelligence, etc.. At the low level, the consideration 
of real-time execution and high accuracy is important. At the high level, the use of AI 
(artificial intelligence) technology becomes crucial. 



CHAPTER 4 


3-D OBJECT LOCALIZATION USING LINE-SEGMENT 

MATCHING 


4.1 Introduction 

Two important factors which will influence the applicability of object localization tech- 
niques in tele-manipulation tasks involving contact with objects are speed and accuracy. 
Gordon and Seering have demonstrated that the successful applications of high-speed and 
accurate localization system in certain automatic assembly tasks can avoid using tradi- 
tional force-controlled motion or precise part fixturing assembly method and therefore will 
simplify robotic operations and improve the flexibility and reliability of performing these 
tasks [47]. Many recognition-localization based object localization algorithms such as [39] 
[45] [76] [112] described in Chapter 3, though having higher levels of intelligence, are not 
suitable for these applications. To achieve high-speed localization, the efforts should focus 
on the following aspects: 

1. Develop fast sensing techniques. 

2. Reduce the overhead for processing sensed data. 

3. Develop fast algorithms to compute the location parameters. Use closed-form for- 
mulas whenever possible. 
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For the purpose of high-speed and accurate object localization, laser range sensing 
systems are the natural choice over other types of range sensors and intensity-image based 

sensors. 

Bastuscheck [8] has discussed the techniques which can lead to the generation of range 
data in real-time. Assuming that a range image is the result of sensor measurement, the 
image consists of 500 X 500 elements and the sensor’s standoff (working area) is about 
40 - 80 inches (1 - 2 m) with 20 inches (0.5 m) depth (working) range, he concluded that 
time-of-hight laser range sensors might be able to generate range images at video rate 
(30 frames of a second). He also found that ratio image range sensors and slit-of-light 
(or multi-slit of light) range sensors, both of which are based on triangulation techniques, 
have the potential to produce 5 to 10 frames of range images per second. Range sensors 
which are based upon either the time-of-flight or triangulation principles and are able to 
generate 5-10 frames of range data per second have been reported recently [82, 91, 92], 
although the number of range readings per frame are all less than 500 X 500. 

The real-time range data generation is only the first step toward high-speed local- 
ization. It has to be followed by a fast data pre-processing step and a fast location 
computation process in order to achieve real-time localization. The task of range data 
processing is to provide necessary parameters for location computation. If the range data 
is from a range image, to extract these parameters the system has to go through a series of 
pre-processing steps which include image-segmentation, region-grouping, feature extrac- 
tion and feature matching. The pre-processing is usually time-consuming. To deal with 
this problem, we can take the following measures: 


• Using a priori knowledges about object location 

Because the high-speed and high-accuracy requirements of locating an object are 
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needed in most cases only when the robot is quite close to that object and is going 
to perform manipulations on it, it is reasonable to assume that a prion knowledge 
about the object position is known approximately before the sensing system takes 
precise measurements on the object. Thus, the direct-localization strategy can be 
employed and the feature matching process is no longer necessary. 

• Using suitable sensing strategy 

Usually only a few features need to be extracted in order to locate an object. For 
example, the extraction of any one of the following groups of matching features is 
enough to derive location (transformation) parameters. 

— Three pairs of independent matching points; 

- Two pairs of independent matching line-segments; 

- One pair of matching line-segment and one pair of matching point, 

— Three line-segments matched on three independent planer surfaces, 

- Six points matched on three independent planar surfaces; 

- Three pairs of matching planar surfaces; 

As a consequence, we do not have to use general range image(s) to obtain needed 
features; certain forms of sparse range data can also be used to provide us enough 
information. Take axis extraction as an example. If the axis of a cylinder surface is 
to be extracted from a range image, the usual steps are first to segment the range 
image into different regions each of which contains only one type of surface, find 
the region corresponding to the cylinder surface, then extract the cylinder’s surface 
parameters from the region, and finally compute the axis of the cylinder surface from 

the surface parameters. 

The same task can also be completed by using a line range sensor. The sensor first 
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projects a plane of a laser beam on the cylinder surface and then takes range readings 
along the intersection of the laser beam and the surface. Finally, the system uses the 
information to compute the cylinder axis directly from these range data. Obviously, 
the second method is much faster than the first one because image-segmentation, 
region-grouping processes are not necessary in the second method. 

Up to now, only limited research directed at fast and accurate object location deter- 
mination has been reported in the literature. 

The algorithm presented by Gunnarsson and Prinz [49, 50] uses spot range data which 
is obtained by using spot sensors to make measurements on the surfaces of an object to 
locate that object. In their algorithm for locating a planar object the measured points 
have to be distributed on at least three surfaces; the measured points on each surface 
should be distributed over as much of the surface area as possible in order to get accurate 
estimation. If a single spot range sensor is used, the sensor has to be moved many times to 
gather necessary data. Even if a multi-spot range sensor is used, it is most likely that the 
sensor will still need to move several times to face different surfaces in order to complete 
the required measurements. While feature extraction and computation are fast, the overall 
localization process is slowed down by the time needed to move the sensor. 

The system developed by Gordon and Seering [48] uses a striped-light and camera 
sensing technique to locate an object. They only studied the case when the object is 
polygon. Again, at least three independent surfaces need to be accessed to obtain enough 

range data. 

In this chapter, a fast and efficient localization algorithm is presented, which is based 
on LINE-TO-LINE matching. Any sensors which are able to make range measurements 
along a line or lines on the object’s surfaces can be used as the sensing device in the 
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algorithm. Such sensors include a line range sensor, a plane of light and camera ranging 
system, or a multi-plane of light and camera ranging system. The measured range data 
will be used to extract line features, which could be boundary edges for planar surfaces or 
axes for surfaces of revolution. The line features are matched to corresponding modeled 
line features. Closed form formulas are used to carry out most computations throughout 
the localization process to speed up the whole localization process. The algorithm requires 
a highly constrained environment. That is, either the environment is a highly-structured, 
or the position relationships among the objects in the environment have previously been 
established approximately. With this assumption, fairly fast and reliable measurements 
can be taken, which in turn will lead to a fast and accurate localization. 

In the next chapter an optimal localization algorithm will be presented. The inputs of 
the algorithm are not limited only to the line features. Featured points (point-to-point 
matching), featured unit direction vectors (VECTOR-TO-VECTOR matching), etc. can also 
be used as the inputs to the algorithm, and there is no upper limit on the number of the 
features inputed. The algorithm will allow the use of redundant features to find a better 
solution. As will be seen, the optimal algorithm is both fast and accurate compared with 


current available algorithms. 
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Measurement rate: 

5 frames/second 

The number of pixels: 

63 

Depth resolution: 

10 micrometer 

Lateral resolution: 

30 micrometer 

Standoff: 

30 mm 

Depth range: 

4mm 

Line length: 

2 mm 


Table 4.1. A triangulation based line range sensor specifications 

4.2 Sensing System 

4.2.1 Sensor requirements 

What characteristics should an ideal range sensing system have? In our previous 
analysis, we have discussed certain important requirements a range sensor should possess 
in order to perform tele-manipulation tasks. We have mentioned that at E-move level 
the localization system should be able to generate an updated measurement in about 
a second. We have investigated the accuracy requirement of performing candidate tele- 
manipulation tasks and concluded that the ranging system should be able to provide 
measurement accuracy up to about 0.01 inches (0.254 mm) in translation and 0.2 degree 
in rotation [96]. We have also listed the advantages of using a line range sensor or a 
plane of light and camera ranging system in reducing the overhead of extracting needed 
geometrical features from sensed data. These discussions should give a good reference for 
design of a range sensing systems. In addition, other factors should also be considered in 
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selecting sensor design parameters, such as ease of use, etc.. Based on the above discussion, 
a list of important performance parameters for an ideal range sensor is given as follows: 

• Measurement dynamic range: 8 to 80 inches (0.2 to 2.0 m) 

• Measurement rate: 5-10 frames per second. 

• Measurement depth resolution: 0.005 inches (0.01 mm). 

• Varying field of view: 35 to 10 degree. 

0 Use multi-line range sensor or multi-plane of light and camera ranging system. 

• Compact size and light weight. 

Though we have not found any range sensor in the market which meets all the above 
requirements, some sensors with specifications which are quite close to these requirements 
have been reported recently . 

A line-range sensor manufactured by CyberOptics Corp. was selected as a prototype 
sensor in our experiments to test the performance of the algorithm. This sensor is a 
triangulation- based laser range sensor and its specifications are given in Table 4.1: [32] 

4.2.2 3-D range sensing fundamentals 


The triangulation based range-finding technique uses a known geometric structure 
between the source of laser beam and the detectors to determine the distance. Figure 
4.1 shows the geometry of such sensors when making a spot measurement. Any position 
along the laser source beam or range can be determined from the position image on the 
detector by using the relationship: 

Z T = h- tanftan-Hy) + 


)] (see[69]) 


(4.1) 
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Figure 4.1. Geometry of a Laser Based Triangulation Sensor. ([69]) 

The line-range sensor uses the same principle. If we assign a right-hand coordinate 
system to the sensor (see Fig.4.2), the coordinates of points of the object in 3-D space 
lying on the projected line may be derived as a function of the readings of the sensor 
detector array. Let 6 be the angle of field of view, k the number of pixels of the detector 
array, and e, the range reading of pixel i, where 1 < i < k. Then the coordinates of the 
point in space corresponding to pixel i are given by 

i(tan * — i-k ) e * 

o < 4 - 2 > 

e, 

This information will be used in the next section as the input to extract Une-segment 
parameters. 



4.3 Line-Segment Feature Extraction 

The object localization process basically consists of two steps: 1) line-segment parameter 
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Figure 4.2. Line range sensor configuration 

extraction from sensed range data and 2) location parameter determination. Two types 
of line- segment are considered in the feature extraction step: 

1. The line-segment to be extracted is a boundary edge of a planar surface; 

2. The line-segment to be extracted is an axis of a surface of revolution. 

Before describing the extraction process, we give a brief discussion on the geometrical 3-D 
feature representation. 

4.3.1 Geometrical representation of 3-D features 

3-D feature representation is a basic problem in object localization. Usually one feature 
can be represented in several different, though equivalent, ways. But usually we will select 
the one which is best for certain applications in terms of efficiency, less calculation error, 
easy computation as the representation of that feature. In the following, we will describe 
the representation schemes of those 3-D features that will be used in our object localization 
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n • x = d 


( 4 . 5 ) 
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Figure 4.4. Representation of a line: (a), vector form; (b). the intersec- 
tion of two planes 

where x = (x,y,z) is any point on a surface, n is the surface normal and d is the 
distance of the plane to the origin. 

• An equivalent form of Eq.(4.5) is 

Ax + By + Cz + D = Q ( 4 - 6 ) 

where A,B,C are directional parameters and at least one of them is a non-zero 
number. 

Lins:- A line in our applications is represented in one of the following two ways: 

• A line is represented by a pair (1 0 , n), e.g., the line has the characteristics that it 
passes through a point lo and is parallel to n. The formula to represent the line is 

lrzlo + fn — oo < v < oo ( 4 - 7 ) 


where 1 is any point on the line. 
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Figure 4.5. The representation of a cylindrical surface 
• A line is thought of as the intersection of two planes: 

t 

A\i 4- Sit/ 4- C\z + D\ = 0 

L : < ' ‘ ' 

Ait + B 2 y + C 2 z + D 2 = 0 

Cylindrical surface : A circuit cylinder is a cylinder with a circular cross section and can 
be specified by a triple (n.xo.r), where n specifies the direction of the axis, *, is the 
translation vector of the axis from the origin and r represents the radius of the cylinder. 
There are other types of cylinders such as elliptic cylinders, hyperbolic cylinders, etc.. 
In our study, we mainly deal with circular cylinders and will call cylinders. A point 
x = (x, y, z) is on the surface of a cylinder if and only if the distance between the point 
and the axis of the cylinder is equal to r. Therefore, the general form of a cylindrical 
surface can be expressed as 

|x - Xo - ((x - Xo) • n)n| = r ( 4 - 9 ) 

Conic sit face t To define a cone, we need to specify a point v to be the vertex of the cone, 
a unit vector u to define the direction of the axis of the cone, and an angle 0 to be the half 
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Figure 4.6. A conic surface is specified by a triple (v,n, A) 

angle of the cone. A point x = (*, y, z) is on the cone if and only if the vector v • x makes 
the angle 9 with n or -n. The general form of a circular cone can therefore be expressed 

as: 

|(x - v) ■ n| = A|x - v| • |n| ( 4 - 10 ) 

where 

A = cos and 9 is the half angle of the cone; 

v = is the vertex of the cone; 

n = (m,n 2 ,n 3 ) is the unit direction vector of the axis; 

x is any point on the cone. 

The absolute value on (x - v) • n is needed to give us both sides of the cone. That is, any 
circular cone can be specified by a triple (v, n, A). 

Spherical surface: A spherical surface can be determined by the center c of the sphere and 
its radius r. That is, if a point x is on the surface, it will meet the equation: 


|x — c| = r 


(4.11) 
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Figure 4.7. The representation of a spherical surface 

4.3.2 Boundary edge parameter extraction 

4.3.2. 1 Boundary edge representation 

A boundary edge of a polygon is usually considered as an intersection of two planar 
surfaces. If the shape of two planar surfaces in the polygon is concave, the boundary 
edge is an internal boundary edge. If the shape of the two planar surfaces is convex, it is 
an external boundary edge. In real applications such as in industry, the boundary edges 
of most parts are rounded. That is, the two planar surfaces are not directly connected. 
Instead, they are connected through a piece of cylindrical surface having certain radius. 
Therefore, we will deal with two types of boundary edges: rounded edges and sharp edges 
and they are modeled in computer as follows: 
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tvpe-oLedge = (rounded, sharp), 
boundary-edge = record of 
plane 1, 

plane2 : plane; 
edge: type-of_edge; 
radius: real; 
end; 

In most cases, when the object is to be displayed on screen in either wire frame form or 
in shaded colors, the rounded-edge feature is not displayed. It is a very useful feature, 
however, in our object localization process. Fig.4.8 gives an example of two different types 
of boundary edges. Fig. 4.8.(a) shows an object with a sharp edge. Fig. 4.8.(b) is its 
range image when the line range sensor is toward the edge of the object. Fig. 4.8.(c) and 
(d) shows an object with rounded edges and a slice of its corresponding range image taken 

toward the edge of the object. 

4. 3. 2. 2 Boundary edge feature extraction 

The objective of boundary edge extraction is to find the parameters of the intersection 
of the two planar surfaces which compose the boundary edge. The process of boundary 
edge feature extraction is first to make a multi-slice of measurements using the line range 
sensor along the boundary edge and then to compute the parameters from these measured 
data. Based upon the sensor’s accessibility on the object’s surfaces, there exist two sensing 
patterns: 1) the sensor can access only one surface of the boundary edge (see pattern (a) 
in Fig.4.9), and 2) the sensor can only access both surfaces of the boundary edge (see 

pattern (b) and (c) in Fig.4.9). 



PRECEDING PAGE BLANK NOT FILMED 
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Figure 4.9. The boundary edge sensing patterns 

For the sharp-type edge, the boundary edge extraction is quite easy. If the sensing 
pattern is (a), e.g., the sensor can only access one surface of the boundary edge, the system 
needs only to detect the boundary edge point from each slice of range readings. This can 
be done by examining the sensor detector’s readings and distinguishing the part of sensor 
cells which has no output from another part of sensor cells which do exist readings. Usually 
the separation point is the corner point and the error is about one pixel. 

If both surfaces are accessible, the sensing system will try to divide each slice of range 
data into two parts based on the changes in slope from their readings. The two parts 
of range data from all slices will then be used to derive two planar equations. The line- 
segment equation is the combination of the two equations. The derivation is a two-pass 
process. During the first pass, all the range data will be used to derive an approximate 
plane equation. A least squares fitting can be used for the purpose. 

Suppose the equation of the planar surface to be determined is ax + by + z = d. We 
need to find the a, b and d from a set of k readings (i<,j where i = !,•••,* and 
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Jb > 3. The following equation can be obtained: 


r 


- 



- 

X\ 

y\ 

l 


a 


-Z\ 

x 2 

V2 

l 


b 

= 






-d 



Xk 

y > * 

l 




-Zk 


The equation can be solved by a closed form formula and have the following solution: 
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(4.13) 


(4.14) 


See Appendix A for the derivation of the solution. 

The approximate planar equation will then be used to reject bad or unreliable data. 
The system will compute the distance between each measured point and the hypothesized 
plane. A maximum acceptable threshold value must be provided. Data having distance 
value exceeding the threshold will be discarded. The remained range data will be used to 
carry out the planar surface parameter computation again using the same formula. 

For a rounded edge, if both surfaces are accessible, the basic strategy is to try to 
separate measured points on each slice which belong to either one of the planar surfaces 


from those points which belong to the rounded part of the boundary edge. The separation 
is still based on the change of slopes. Fig.4.11 shows a part of a slice range data measured 
on a rounded edge when both sides of the surfaces are accessible. 



Range Readings (Z) 
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Figure 4.10. A part of a slice range data measured on a rounded edge. 

If only one surface is accessible, the derivation process is not as easy as in the sharp- 
type edge case. But by using the geometric property of the boundary edge, this task can 
still be accomplished. The procedure is first to divide the range data of each slice into 
two groups, one belongs to the surface, another belongs to the rounded edge and then to 
compute the surface parameters from the two groups of range data. A planar equation 
can be found from the first group of range data. The second group of range data can be 
used to find the axis parameters of the rounded edge (cylindrical surface). Usually, the 
intersection of the plane of a laser beam and a cylindrical surface is an ellipse. The axis 
parameter determination can be done by using one of the methods given in Section 4.3.3. 




Range Readinds (Z) 
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Figure 4.11. The range data where only one planar surface is accessible. 

Now, we have computed one surface equation and the location of the axis of the 
rounded edge. From the modeled description of the boundary edge, we know the radius of 
the rounded edge and the angle between the two surfaces. These conditions are sufficient 

to derive the location of the boundary edge. 

Fig.4.10 gives an example of the range image where only one planar surface of the 
boundary edge is accessible. By using the prior knowledge about the boundary edge, we 
are still able to extract the corner point from the data. 

4. 3.2.3 Plane parameter error estimation 


The estimation is made by deriving the standard deviations of the parameters of the pianar 
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surface. Tlie standard deviation for a set of measured range data z,,t - 1 • • • k is 


<?z = 


W 

k - 1 


(4.15) 


where <5z; = z t - - z and 2 is the most probable value of z;’s and is equal to 

* = (4 - 16) 

* ,=i 

According to the error theory [9], if a variable v is derived from a set of variables 
u,- t = 1, • • • , k and has the function relationship 

v = V(ui, •••,«*) ( 4 -l 7 ) 

then, the propagation of error from tx, t = 1, •••,* to u is expressed by the Mowing 
formula: 

» = 1 OU ' ,‘=1 j=l*t 3 

where p u , U) is the correlation coefficient of variables u t and uj and is determined by the 
formula: 


If all the variables are 
plified as 


o = ( 4 . 19 ) 

' (A: - 1 )<t u ,< 7 Uj - =1 

independent and uncorrelated, the equation (4.18) will be sim- 



(4.20) 


In our case, because variables a,b and d are dependent on z;,i = l,”, fc and their 
relationships are determined by the equation (A.7), the standard deviation for parameters 
a,b and r can be explicitly expressed. Using (A.7), the variable a is expressed as 


a = f(z u ---,z k ) 

k 

= £(WW.- + Wl2Vi z i + W\3Zi ) 

i=l 


(4.21) 


(4.22) 
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If z,’s are independent, e.g., p Zi z, — then 


V7 °f 

= 


t = l 

k 


= YfWn xi + W u yi + WxzYoi 

1 = 1 

Furthermore, if all *’s have the same distribution property, we have 

*1 = °icb ( w + Wi2y ‘ + ^ i3)2) 

t=i 

The expression (4.25) can be further simplified. 

k k 

al = <j\ £(W n x, + W l2 y; + W 13 ) + ^ 
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(4.23) 
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(4.25) 
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(4.27) 

(4.28) 
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Because the matrix W is the inverse of M, e.g., MW = I. Using the property of 
inverse matrix, we have WM = I- That is 

< 

1 if i = 3 


j=i 


(4.30) 


0 if i 4- j 


Therefore 


a I = a 2 .Wn 


(4.31) 


Similarly, crl,o 2 _ d can be derived: 




= aiWn 
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It should be noted that during the above derivation process, we do not make any assump- 
tion about the types of the distribution of the range data. That means, the result can be 
used no matter how the measured range data are distributed, Gaussian or non-Gaussian, 
as long as all the measured points have the same distribution. 

The standard deviation of surface normal n and distance d can also be derived from 

equations (A.8) to (A.U) and (4.20). That is, 


Similarly, 
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In matrix form, they can be expresses as 
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And finally, from s = \/a 2 + 6 2 + 1, we have 

2 _ a 2 fra + l 
a 2 + 6 2 + 1 


(4.34) 
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Figure 4.12. The intersection of a plane and a cylinder is an ellipse 

4.3.3 Axis parameter extraction of a cylindrical surface 

The most frequently seen surfaces of revolution in industry include cylindrical surfaces, 
conic surfaces and spherical surfaces. The extraction of cylinder parameters is the one 
most studied by researchers. The cylinder parameters can be extracted from range images 
[53], intensity images [25], or line-range finders [20, 46]. Basically, there are two methods 
of extracting cylinder parameters by using line range sensors, which are called one- scan 
and two-scan methods, respectively, depending on how many line(s) of range data are used 
to do the extraction. 

4.3.3. 1 One scan method 

When a laser line beam is projected on the surface of a cylinder, the intersection of the 
projection plane and the cylindrical surface is an ellipse. The range data obtained from 
the projection can be used to derive the ellipse parameters. There are several algorithms 


v 

Sensing ^ 
Frame ^ 
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Figure 4.13. The extraction of axis from two scans ([46]) 


which can do ellipse fitting [21, 86, 85, 99], Once the ellipse is fitted to a set of range 
data, the cylinder axis can be found very easily because of the well known characteristics 
of the ellipse (see Fig. 4.12): 1) Its center lies on the axis of the cylinder; 2) Its conjugate 
dimension is equal to the radius of the cylinder; and 3) The ratio of its principal dimension 


to its conjugate dimension is the cosine of the angle between the axis of the cylinder and 
the projection plane. 

Thus, only one scan of the line range sensor on the cylinder surface is enough to solve 
for cylinder axis parameters. The accuracy will be improved if multiple scans are used. In 
this case, the final result is obtained by averaging all the results calculated from each scan. 
This method also has its problem. Most algorithms which have been used to derive ellipse 
parameters are iterative or recursive and therefore not efficient and sometimes converge 
very slowly [2, 21, 85, 86, 99, 118]. 
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4. 3.3. 2 Two scan method 

In the two-scan method [1, 46], two scans on the cylindrical surface are needed to 
extract axis par am eters. The extraction consists of two steps. The first step is to use 
the two scans to compute the direction of the axis of the cylinder; the second step is to 
compute the position of the axis. 

Step 1. Find the Direction of the Axis: 

This method uses the geometric properties of a cylinder. That is, given two scans A 
and B on a cylinder, select the pairs 01,61,02,62 so that line segments 0161 and 0262 are 
parallel (see Fig. 4.13). Then the line segments might either be parallel to the cylinder 
axis, or they might not. If they are not parallel to the axis, the four points 01,61,02,62 
are the only points that the two line segments 0161, 0262 intersect with the surface of the 
cylinder. Therefore a third scan C can be used to test the parallelism of the two lines and 
the cylindrical axis. This can be done by extending one of the two parallel lines until it 
intersects with the plane of a the third scan and testing whether the intersection point is 
on the ellipse defined by that scan. If it is, the two lines are also parallel to the cylindrical 
axis. Otherwise, they are not. [1, 46] have given detailed discussions about the validity 
of the argument. [46] has described an algorithm. The basic idea of the algorithm is as 
follows: 

Pick two points on one scan, 0^(1 = 1,2); the points should be widely separated 
on the scan. Try to select two points 6,(i = 1,2) on the second scan so that 
the dot product of the two unit vectors = 1,2) from a, pointing to 6; is 

close to 1. Store the two pair of points. 

Find several pairs of points by repeated executing the procedure. 

The above procedure will be performed on two other scans. If all the vectors give 



Figure 4.14. Projection of a circular cylindrical surface 

roughly the same direction and are parallel to the axis of the cylinder, then the axis 
direction will be the average values over all the vectors. 

Step 2. Find the Axis Displacement: 

The axis displacement can be obtained by using a projection method. Once the axis direc- 
tion is known, ah the range data will be projected to a plane which is perpendicular to the 
axis. The projection matrix is made in such a way that the z axis in the coordinate system 
is rotated to the direction of the normal vector n of the plane. All the range data will 
then be projected on the plane by multiplying M with these data points and taking only 
x and y components of the multiplication. If the axis direction of the cylindrical surface 
is correct, the projected data to the plane will form an arc (see Fig 4.14). The rotation 
matrix has the property that it is an orthonormal matrix and that Mn = (0 0 1) T . The 
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Figure 4.15. The error between the area of a circle and the measured area 
of the circle 

matrix is not unique. One of them has the form. 


M = 



Tlx 



0 

-9 

n z 


(4.39) 


where g = + n 2 y . 

The arc will then be fitted in a circle. During the process, the circle’s center and radius 
will be calculated. In fact, we are only interested in the circle’s center. 

Traditional methods use either iterative method [75] or nonlinear optimization methods 
[53] to derive the circle parameters. While the accuracy of these methods is promising, 
the execution speed will be slowed down quite markedly as the number of measured points 
increases. To speed up the circle parameter calculation, a closed form formula is used. 
The formula is based on least squares optimization, which tries to minimize the squares 
of the errors between the area of the circle and the area of the circle defined by the center 
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of the circle and the measured point. 

Suppose that the projected range data are expressed as 

{5 = {xi,yi)\i = 

and the desired circle is located at c = (xo.lto) with radius r, The measured area from the 

measured point (n, Vi) is i((*i - *o) 2 + (vi - Vo) 2 ) “<* the area of the circl€ wi " be 
The error between the two areas is (see Fig. 4.15) 

e,- = - x 0 ) 2 + ( Vi ~ Vo ) 2 ) - 7rT ' 2 ( 4,4 °^ 

The traditional least squares optimization can be used to minimize the error function. 

£ = ( 4 - 41 ) 

t'=l 

and the circle parameters can be solved from it by a closed form formula. See Appendix 
B for the detailed derivation. 

The two dimensional coordinates of the center will then be back-projected in the 
original 3-D coordinate system and be used as the displacement of the axis. 

The efficiency of the closed form formula can be clearly seen in Fig. 4.16, where the 
execution speed of the closed form algorithm for different number of sampled points is 
plotted against the execution speed of the iterative algorithm described in [75]. The 
comparison of the two algorithms was carried out on a SUN-3/50 computer without using 
math-coprocessor. The chart shows the following facts: 

1. The closed formula algorithm is about 10 times faster than the iterative algorithm. 
When the number of sample points are 100, their execution times are 0.2 and 2.1 
seconds, respectively, when the iterative error = 0.001. Their execution times in- 
crease to 0.84 and 8.01 seconds, respectively, as the number of sample points reach 


400 . 



Time (second) — for 50 Trials 
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Execution Times of Two Algorithms 



Figure 4.16. A comparison of the execution speeds of the two algorithms 

2. The execution time for the iterative algorithm will increase considerably when higher 
accuracy is required. But the execution speed of the closed formula algorithm is less 
influenced by the accuracy requirement. This is because higher-accuracy means 
more loops for the iterative algorithm. The average execution time will increase 
about 20% when the error bound changes from 0.001 to 0.0001 and increase another 
10% when the error bound becomes 0.00001. 

Experiments have shown that the two algorithms basically have the same level of 
localization accuracy. See section 4.5 of this chapter for more detailed demonstration. 
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Figure 4.17. The intersection of a plane and a cone: (a) general cases; (b) 
ellipse and its axis 

4.3.4 Axis parameter extraction of a conic surface 

In contrast to the study on cylinder parameter extraction, very little research work on 
conic parameter extraction could be found in literature. In the following, we will discuss 
how the two methods described in the last section can be extended to the case of extracting 

conic parameters. 

4.3.4. 1 One scan method 

In general, the intersection of a conic surface and a line of laser beam produces a 
quadratic curve. The shape of the curve could be a hyperbola, a parabola, an ellipse, a 
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circle, or a straight line depending on the relative position between the light plane and 

the cone (see Fig. 4.17(a)). In most cases, the shape of the curve is an ellipse. If the 

intersection curve is an ellipse, it has the following important property: 

The intersection point between the axis of a cone and the light plane is always 
on the principal axis of the ellipse (see Fig. 4.17(b)). 

Section C.l of Appendix C gives the detailed discussion about this property. 

Now, suppose a conic surface is sensed by a laser line range sensor. Because the 
approximate position of the cone is known, it should be no difficulty in controlling the 
sensor’s position so that the intersection of the laser beam plane and the conic section is 
an ellipse curve. The ellipse parameters can be extracted from the range readings along 
the curve by using the algorithms discussed in the previous section. Now we have the 

following question: 

Can we extract the conic axis parameters by measuring only one slice of line- 
segment along the conic surface? 

The answer is YES, if we know the half angle 6 of the cone and know that the intersection 
curve is an ellipse. Section C.2 of Appendix C gives the formula of carrying out the required 
computation and describes the derivation of the formula. Again, multiple measurements 
will improve the accuracy of the axis extraction. 


4.3.4. 2 Three scan method 


The three-scan method we use for axis parameter extraction is based on the following 
observation: 

Given three scans on a conic surfaces, which axe obtained as the intersections 
of three different laser beam planes with the conic surface, we select any two 
scans from them. If a pair of points on these two scans have the property 
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Figure 4.18. Three scan method to extract the axis of a conic surface. 

that the line formed by connecting the two points passes through the vertex 
of the cone, then the line must intersect with the third scan. We call this line 

a generator of the cone. 

The extraction process, like the twtxscan method for cylinder axis extraction, again con- 
sists of two steps: axis direction extraction and axis position extraction. 

Step 1. Find the Direction of the Axis: 

The extraction of axis direction uses the geometric property of a cone that the angle 
between the axis of the cone and any generator of the cone is always the same. That is. 
suppose n, and n 2 represent the unit direction vectors of two generators of the cone and 
„ represent the the unit direction vector of the axis of the cone, we have 


e.g., 


n • (ni - «2) = 0 


(4.43) 
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Input: Sets of measured points taken from three scans on a conic surface 
A = {a,- = = l,-"> m i}; 

B = {b, = (b ix ,bi v ,b, z )\i = 

C = {c, = (ci,,c^,Ci,)|i = 

Output: A pair of points (ai,b fc ). 

Begin 

01 Find a best-fit plane C from the set of measured points c x ; 

02 Select an arbitrary point on scan A, ai; 

03 For i := 1 to m 2 do 

04 Compute the intersection point p, between the line a x 6 t and the plane <; 

05 Find the distance di between p^ and scan C and store the value; 

{* After steps 03-06, a distance curve about di is obtained *} 

06 Smooth the distance curve ; 

07 Find the point b^ on the the curve with minimum the distance to Scan C; 

End 1 

Figure 4.19. Three scan algorithm to find a generator of a cone. 

This means that the vector n is perpendicular to vector n x - n 2 . Given three different 
generators n = 1,2,3, we can derive the the axis direction vector by the formula: 

_ ( n i ~ 1*2) x (n 2 x n 3) (4.44) 

n - |(m - n 2 ) x (n 2 x n 3 )| 

and it is unique. 

The algorithm for finding a generator of a cone can be described as follows: 

Three consecutive scans are taken on a conic surface and they are named A, B 
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and C from the bottom scan to the top scan respectively (see Fig. 4.18). Pick 
a point on scan A, a x . Try to select a point b, on scan B so that the line 
formed by ( ai ,b,) is close to scan C (see Fig4.19 and 4.20 for algorithms). 

One potential problem in using the above algorithm is that the existence of noise can 
lead to a false local minimum distance value between the line and the scan. The real 
implementation of the algorithm actually computes more points. The distance values 
should tend to approach minimum and then start increasing. The algorithm will smooth 
the distance curve and find the minimum point from the smoothed curve and take this 
point as the required b,. 

An implementation of the algorithm is described in Fig. 4.19. In that algorithm, the 
step 05 - “find the distance d • between Pi and scan C" may require significant time to 
compute. We can do better if the three scan lines are parallel. In real applications, when 
a multiple-scan sensor is used, the scan lines are usually parallel. Even if only a single 
scanner is used, to make parallel scans is not very difficult. In this case, if we use same 
sensor coordinate frame described in Fig.4.2, then the three scan planes have the equations 
y = Wi ,i = 1,2,3. For convenience, the scan plane A can be assigned to y = 0. After 
such an assignment, the step 01 of the algorithm in Fig.4.19 is unnecessary. In addition, 
if the x values are used to represent the sensor cell’s positions, their values are integers 
which will further simplify the implementation of the algorithm. A revised version of the 
algorithm is shown in Fig. 4.20. 

Among these three scans, we can arbitrarily assign any scan as scan A, scan B or scan 
C. But in practice, we usually select the scan which is closest to the bottom of the cone 
as scan A, the middle scan as scan B and the scan which is closest to the vertex of the 
cone as scan C. There are two obvious advantages of doing such kind of assignment: 
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Input: Sets of measured points taken from three parallel scans on a conic surface 

A = {a; = (ai„0,O|i = 

B = {b, = 

C = (c,- = m 3}'> 

Output: A pair of point (ai,b fc ). 

Begin 

01 Select a point on scan A, ai; 

02 For i := 1 to m 2 do 

03 Compute the intersection point p< between the line and the plane y = w 3 ; 

04 If a measured point c, on C scan can be found so that c ix is equal 
to round(p, I ), go step 06; 

35 Go to next loop; 

06 Find the difference Az, between p, 2 and a, and store the value; 

07 Go to next loop; 

08 Smooth the Az,’s data; 

09 Find a point (x 1 ,Az fc ) on the the smoothed curve, which is the extreme 
point of the curve; 

10 Find the point b 0 corresponding to that point; 

End 


Figure 4.20. Revised three-scan algorithm: all scans are parallel. 
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1. By using such an assignment, a little change on 6, will result in big change in A*. 

As a result, the distance curve will have a sharper shape and it is easier to find the 

required point; 

2. Relatively few intersection points p, computed from step 03 have corresponding c 

points. As a result, relatively few Ar, values will be stored and less effort wdl be 

spent on processing it. 

A computer simulation was performed to compare the performance of two different 
types of assignments. A cone having a half angle of 30" is placed at a position with 
respect to the sensor frame such that the vertex’s position is (15,15,15) and the axis 
direction vector of the cone is (2,3,4). Three scan planes are , = 0, y = 2 and y = 5, 
respectively. All the units are in centimeters. Each scan has 255 measurement points, 
which have covered about one third of the intersection ellipse. Fig.4.21 shows the distance 
curve using our preferred scan assignment. Fig.4.22 shows the resulting curve of using 
different assignment, where the scan B is assigned to the scan closest to the vertex of 
the cone and the scan C is assigned to the middle scan. The measurement noise has a 
normal distribution with a standard deviation of 0.3. In Fig.4.21, only about one half of 
the intersection points have their correspondence. On the contrary, all the intersection 
points can find their correspondence in Fig.4.22. The difference in the sharpness of the 
resulting curves is also obvious. 

Two methods have been tested for A* data smoothing. One method is called curve 
fitting. In this method, all the measured data are used to fit into a single quadric curve 
in the form of z = a + 6* + c* 2 . In the second method we do not try to use only one curve 
to fit all the data. Instead, each portion of data will be fitted into a quadric curve, with 
each quadric curve having different parameters. The new curve as a whole is smoothed. 
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Figure 4.21. Distance curve using the preferred scan assignment. 

We will take the extreme point of the smoothed curve an the required point. Appendix D 
gives the detailed description of the two methods for smoothing. The two methods give 
similar results. No matter which method is used, one thing should keep in mind, namely, 
the objective of data smoothing is to determine the point which will let us find a generator 
of the cone. Therefore, it is important to observe the shape of the curve as whole through 

smoothing, not to find the exact value at each point. 

The algorithm just described is quite insensitive to sensor measurement noise and 
roughness of the object surface. This is because the point which wiU lead us to find a 
generator of the cone is obtained by observing the tendency of the distance curve instead 
of simply computing a minimum or maximum point from the data set. 

To derive the axis direction, at least three generators are needed. In real applicatrons. 
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Figure 4.22. Distance curve using a different scan assignment, 
we usually compute more than that and average the results to obtain a better approxima- 
tion. 

Step 2. Find the Axis Displacement: 

The axis displacement computation procedure for a conic surface is the same as the pro- 
cedure for computing the axis displacement of a cylindrical surface. Once the conic axis 
direction is derived, the projection plane and projection matrix can be found. Instead of 
projecting range data, all the generators will be projected onto that plane. The result 
of the projections is a set of two-dimensional straight lines. Usually, these lines are not 
parallel. Thus, each pair of lines will have a cross point. If n(n > 3) lines are projected, 
the number of cross points is These cross points are distributed on a very small 

circle area. The center of the area can be derived from these cross points by using the 
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closed-formula circle parameter estimation algorithm described in last section 


4.3.5 Axis parameter extraction of other surface of revolution 

The three-scan method can in fact be used for any cylindrical type of surfaces, such as a 
circular cylinder, elliptic cylinder, hyperbolic cylinder and so on. The axis direction vector 
can be determined in the manner described in Fig. 4.20. All the measured points will then 
be projected onto a projection plane in order to tod the axis displacement. Except for 
the case of a circular cylinder where a closed form formula is available to find the center 
of the projected arc, an optima] quadric curve fitting is needed to find the quadric curve 
parameters. For other types of surfaces of revolution, we have to analyse the geometrical 
properties in order to apply proper methods. 

4.4 Location Determination 


Suppose two non-parallel line-segments, f„ i = 1,2 , either boundary edges or axes or a 
combination of them, have been extracted, and are expressed as 

U-. h = io. + Crt, i — 1,2 -oo<v<+oo (4-45) 

Here i„. are the vectors from the origin of the sensing coordinate system perpendicular to 
and intersecting the lines the line-segments be on, and * are the uni. direction vectors 
of the line-segments viewed from the sensing coordinate system. Their corresponding 
line-segments /, in object model are expressed as 


h : h = lo, + vni 


i = 1,2 


- OO < V < +°° 


(4.46) 
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where lo, are the vectors from the origin of the object coordinate system perpendicular 
to and intersecting the lines on which the line-segments lie, and n, are the unit direct, on 
vector of the line-segments. lithe two pairs of measured line-segments Oi.Ij) and modeled 
line-segments (1„1,) have the same angle and the same distance, a unique transformation 
matrix can be derived from them, which will transform the modeled line-segments to their 
corresponding measured line-segments. That is, the location determination is to And a 
transformation matrix T such that 

I, = Tl, t = 1,2 ( 4 - 47 ) 

In the case where they are not well matched, more matching pairs must be found in order 
to derive an optimal solution, which is the task of Chapter 5. 


4.4.0. 1 Determining the Rotation Parameters 


The rotation can be represented in several ways. In our case, because two pairs of match- 
ing vectors are given, it is natural to use the properties of vector algebra to denve the 
rotation parameters. Thus the best way of representing the rotation is to use the axis- 
angle representation method. The rotation axis r can be easily derived by using the fact 
that the rotation axis should be perpendicular to both (n^) and (n 2 -n 2 ) and has the 

following form: 

__ (ni - ni) x (n 2 - h 2 ) ( 4 . 48 ) 

r “ ||(m - hi) x (n 2 - n 2 )|| 

to within an ambiguity of r radian. 

Once the rotation axis r has been computed, the rotation angle can be determined by 


using the following formula: 


n, • hi - (r • hQ 2 
1 - (r • hi) 2 


cos a = 


(4.49) 
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Figure 4.23. Two line-segments are in the same plane 

Given values for r and a, the rotation submatrix of the transformation matrix can be 
determined. For a detailed derivation, see Appendix E. 

4.4.0. 2 Determining the Translation 

When considering the determination of the displacement, we have to deal with two possible 
cases: 1) the two line-segments are located in the same plane; and 2) they are in different 

planes. 

In the first case (see Fig 4.23), the intrinsic geometric properties of the line-segments 
are used to derive the formulas. From the given conditions, the intersection point p of 
the two lines emanating from the line-segments and the parameters (n„d) of the plane 
determined by the two line-segments, all expressed in the object coordinate system, can 
be calculated. Also from Fig.4.23, we have the following equation: 


o = p + ani + bn 2 - dn. 


(4.50) 
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Figure 4.24. The cross point of two line-segments 
The parameters a and b can be derived by using the properties of vector algebra: 


a = {(dn, - p) - bn 2 ) • nx 

( dn, - p) • n 2 - ((dn, - p) • nx)(nx • n 2 ) 

^ = 1 - (n x • n 2 )^ 

Using the constants d,a,b and the extracted line-segment parameters, the translation 
vector t is given by 

t = p + aiii + bn? — dh 3 (4-51) 

where the parameters n„ n s , n„ and p are the same parameters corresponding to 
E q.,4.50) but are observed from sensing coordinate frame, and can be computed from 
the the extracted line-segment. The correctness of the Eq.(4.51) is based on the fact that 
during any transformation, the relative position and orientation of vectors p, n,, nj and 

n, are unchanged. 

For the second case where two line-segments are in different planes, we can first find 
the two cross points of a line which is perpendicular to both of the two line-segments and 
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has the shortest distance. See Fig.4.24. Once the cross points have been found, we can 

use the properties of vector algebra to compute the displacement vector, going through 

the same procedure as described in the the first case. 

If the modeled line-segments are given as Eq. (4.46), the coordinates of cross points p x 

and p 2 can be derived by computing the corresponding v values of each of the line-segments 

represented by Eq. (4.46). Thus, 

(loi — 1(h) • n i ~ ( n i ' n 2)(loi ~ ' n2 (4.52) 

1/1 “ (n x • n 2 ) 2 - 1 

and 

— (lo t ~ (o2) • n 2 + (m • n 2 )(loi - l<h) ' n t (4.53) 

1/2 ~ (ni • n 2 ) 2 - 1 

Because the two line-segments are non-parallel, (m • n 2 ) 2 - 1 wiU not be equal to zero and 
therefore we do not need to consider the overflow problem here. 

The coordinates of the same two cross points which, however, are observed from the 
sensing system \ and p 2 can be derived by using the same forms of Eq.(4.52) and (4.53). 
What we need to do is to change all the vector notation from modeled notation into tilde 

notation. 

4.5 Experiment Results 


4.5.1 Accuracy Performance on the Circular Arc Parameter Estimation 

Computer simulations have been carried out on a SUN computer to compare the accuracy 
performance of the circular arc parameter estimation between the closed formula method 
and the iterative method [75]. After each simulation process, the estimated circular center 
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and radius are calculated. Because we are only interested in the the circular center, the 
results of circular radius estimation are not shown here. In fact, only the estimations of 
center’s x-coordinate are demonstrated in the following figures. 

The simulation results are presented in Fig. 4.25 and Fig. 4.26. In Fig. 4.25, all the 
data points are evenly taken from an arc of 180° and radius 20.0 units, and the center of 
the arc is placed at the origin (0.0, 0.0) of the 2-D coordinate system. The data points 

are corrupted with a normal distribution along both X and Z axes. Three different levels 
of corruptions are used in the simulation, e.g., with a standard deviation of 0.1, 1.0 and 
2.0 respectively. The number of measured data points range from a minimum of 3 points 
to a maximum of 400 points. For each given set of data points and each corruption level 
setting, 25 trials are performed for both of the two algorithms. The results are then used 
to compute the standard deviations on the center’s location. The computed standard 
deviations are drawn on the figure. Fig. 4.26 shows the simulation results performed on 
an arc of 60 a . 

From the two figures, we have the following observations: 

• When the input noise is small, the iterative method provides a better estimation. 

• As the input noise increases, the difference on accuracy performance of the two 
algorithms becomes smaller. They have the same level of accuracy performance 
when the input noise reaches to the level of standard deviation 1.0. 

• If the input noise increases further, the closed formula gives a better result. 

• In any case, the output error level is much less than the input noise level. For 
example, when the input has a noise level of standard deviation 1.0 and contains a 
total number of 100 measured data points on an arc of 180°, the output error only 
has a standard deviation of 0.07 for both of the algorithms. 
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In practice, the selection of a best algorithm is depended on many factors such as the 
required execution speed, the roughness of object surfaces, sensor quality, the number of 

measured points etc.. 


4.5.2 The Accuracy Issues of the Revised Three-Scan Algorithm 


The key step of the cone parameter extraction is the extraction of a cone’s generator. 

We have proposed a three-scan algorithm in Section 4.3.4.2 for this purpose. A revised 

three-scan algorithm in which all three scans are parallel was illustrated in Fig. 4.20. In 

this section, computer simulations are set to further demonstrate the performance of the 

algorithm under different conditions. The cone we used in the simulations has the same 

parameters: a half angle of 30°, a vertex position of (15, 15, 15). The scan A and C are in 

planes Y = 0 and Y = 5, respectively. The simulation process is as follows: 

For each scan B position and each simulated measurement noise level, 30 trials 
axe performed. During each trial, the simulation program first produces a set 
of normal-distributed measurement data, and then runs the revised three-scan 
algorithm. The output of the algorithm is a point b< on scan B such that the 
line represents a generator of the cone. Suppose the ideal point is b 0 , the 
erroron pixel will be 6e, = |i - o|. After the 30 trials, the average error on 
pixel position is computed and stored in the table. 

Table 4.2 shows the accuracy performance of the algorithm with different measurement 
noise levels and different scan B positions. The cone direction vector is (2,3,4). Each scan 
covers about one third of the intersection ellipse and consists of 250 pixels. The first column 
of the Table 4.2 lists the simulated measurement noise levels; the next three columns give 
the average computed pixel position errors with the scan B being positioned at planes 
Y = 1, Y = 2 and Y = 3, respectively. The table clearly tells us that the noise levels 
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have no influence on the output accuracy, e.g., the algorithm is noise-insensitive. In fact, 
the table even shows a slight improvement on the output accuracy when the measurement 
noise is increased from Sd = 2.5 level to Sd = 5.0 level. The table also shows that the 
pixel position errors become smaller as the middle scan B moves closer to the bottom 
scan A. If the pixel position errors are converted into corresponding generator’s direction 
errors, the actual average angular errors on the generator’s direction are 0.369°, 0.386 
and 0.513° for scan B at planes Y = 1, Y = 2 and Y = 3, respectively. 

Table 4.3 shows the accuracy performance when all the scans are perpendicular to 
the axis of the cone, e.g., the cone direction vector equals to (0,1,0). The algorithm s 
noise-insensitive property is also demonstrated in the table. The table also shows that the 
accuracy becomes worse as the middle scan B moves closer to the bottom scan A. 

Table 4.4 and Fig. 4.27 present the detailed simulation results taken from a set of 
randomly produced data points. Table 4.3 lists the cone and scan parameter settings 
and gives a partial listing of the X values corresponding to the pixels on scan B and 
C, their corresponding theoretical Z values and the corrupted Z values. A pixel on scan 
A (ellipse 1) is selected as the a! in the three-scan algorithm. Fig. 4.27(a) shows the 
theoretical distance curve and the distance points computed from corrupted Z values; and 
(b) shows the smoothed distance curve after the three-scan algorithm is implemented. In 
this simulation, the final selected pixel is coincident with the ideal pixel, e.g., no error. 

Fig. 4.28 presents the simulation result taken from another random set of measured 
data when the scans are perpendicular to the axis of the cone. After the execution of 
the algorithm, the output (the final selected pixel position) is three pixels away from the 
ideal pixel position. In the figure, the pixel positions are converted into corresponding x 
coordinates. Thus, it is hard to see the exact pixel position error directly from the figure. 


Standard Deviation 
for X 
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O dosed formula (Sd=2.0) O dosed formula (Sd=1.0) a dosed formula (Sd=0.1) 

□ iterative (Sd=2.0) X iterratlve (Sd=sl.O) + Iterative (SD=0.1) 



Figure 4.25. (a). Accuracy Comparison of Two Algorithms (Arc- 180 de 
gree); (b). Enlargement of (a) for the Input Sd=0.1 Case. 









Standard Deviation 
for X 
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Accuracy Comparison for Input Sd=0.1 
A closed formula (Sd =0.1) * iterative (Sd=0.1) 
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Figure 4.2G. (a). Accuracy Comparison of Two Algorithms (Arc — GO de- 
gree); (b). Enlargement of (a) for the Input Sd=0.1 Case. 
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Cone direction vector: (2, 3, 4) 
Scan A : Y=0, Scan C: Y=5 


Input 

Average Errors on Pixel Position 

Sd 

Y=1 

Y=2 

Y=3 

0.1 

3.0000 

6.0000 

10.4000 

0.5 

2.7667 

5.9333 

10.5000 

1.0 

2.8667 

5.8333 

10.5333 

2.5 

3.2000 

5.4667 

10.4333 

5.0 

L .3667 

4.7667 

9.9333 


Table 1.2. Accuracy performance of the three-scan algorithm with cone 
direction vector (2,3,4) 
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Cone direction vector: (0, 1, 0) 
Scan A : Y=0, Scan C: ^ =5 


Input 

Average Errors on Pixel Position 

Sd 

Y=1 

Y=2 

Y=3 

0.1 

2.7333 

5.9667 

5.7000 

0.5 

7.9667 

4.6667 

5.8333 

1.0 

4.5000 

5.2000 

2.6667 

2.5 

5.3000 

5.6667 

4.9667 

5.0 

5.9000 

5.7333 

4.6667 


Table 4.3. Accuracy performance of the three-scau algorithm with cone 
direction vector = (0, 1, 0) 
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standard deviation: 1.0 
noise factor: 0.1 
ellipse 2 plane: Y-l 
ellipse 3 plane: Y-5 
number of points: 250 
start angle :35 
ending angle: 160 
cone: 

vertex: 15 15 15 
direction vector: 234 
half angle: 30 


ellipse 2 data: (pixel 108-157) 

x model z measured z 


108 

-9 

.577 

0 

.984 

1 

.895 

109 

-9 

.870 

0 

.734 

0 

.885 

110 

-10 

.164 

0 

.482 

-0 

.344 

111 

-10 

.458 

0 

.228 

0 

.789 

112 

-10 

.752 

-0 

.030 

-0 

.197 

113 

-11 

.045 

-0 

.289 

0 

.669 

114 

-11 

.339 

-0 

.551 

-1 

.194 

115 

-11 

633 

-0 

.815 

-0 

.190 

116 

-11 

.927 

-1 

082 

-2 

.471 

117 

-12 

.220 

_ i 

351 

0 

.766 

118 

-12 

514 

-1 

622 

-0 

.932 

119 

-12 

808 

-1 

896 

-0 

064 

120 

-13 

101 

-2 

172 

-3 

174 

121 

-13 

395 

-2 

450 

-2 

309 

122 

-13 

689 

-2 

731 

-2 

413 

123 

-13 

983 

-3 

013 

-2 

312 

124 

-14 

276 

-3 

298 

-3 

069 

125 

-14 

570 

-3 

586 

-4 

905 

126 

-14 

864 

-3 

875 

-3 

365 

127 

-15 

158 

-4 

167 

-3 

888 

128 

-15 

451 

-4 

460 

-3 

410 

129 

-15 

745 

-4 

756 

-6 

137 

130 

-16 

039 

-5 

054 

-4 

768 

131 

-16 

333 

-5 

354 

-5 

084 

132 

-16 

626 

-5 

657 

-6 

384 

133 

-16 , 

.920 

-5. 

, 961 

-5. 

,691 

134 

-17 , 

.214 

-6. 

,268 

-7 , 

,767 

135 

-17 , 

.508 

-6, 

, 576 

-6, 

,029 

136 

-17 , 

.801 

-6 , 

.887 

-8 , 

,497 

137 

-18 . 

.095 

-7 , 

, 199 

-8, 

,500 

139 

-18 . 

.389 

-7 , 

,514 

-7 , 

,346 

139 

-18 , 

. 682 

-7 , 

,831 

-6. 

,269 

140 

-18 , 

. 976 

-8, 

,150 

-6. 

,549 

141 

-19 . 

.270 

-3, 

,470 

-11 , 

.643 

142 

-19 . 

.564 

-8, 

,793 

-9. 

,322 

143 

-19. 

.857 

-9, 

,118 

-9. 

,334 

144 

-20 . 

.151 

-9. 

, 445 

-9, 

,840 

145 

-20 . 

.4 45 

-9. 

,773 

-10 . 

,132 

146 

-20 . 

,739 

-10 . 

,104 

_a , 

,314 

147 

-21. 

.032 

-10. 

, 437 

-11. 

,406 

148 

-21. 

.326 

-10, 

,771 

-10. 

,565 

149 

-21 . 

. 620 

-11. 

,108 

-12. 

,740 

150 

-21 . 

. 914 

-11. 

,446 

-12. 

,740 

151 

-22 . 

,207 

-11. 

,787 

-11. 

,421 

152 

-22. 

,501 

-12. 

,129 

-12 . 

939 

153 

-22. 

.795 

-12. 

473 

-12. 

848 

154 

-23 . 

,089 

-12. 

819 

-12 . 

366 

155 

-23 . 

,382 

-13. 

167 

-14 . 

125 

156 

-23 . 

,676 

-13. 

517 

-13. 

553 

157 

-23 . 

,970 

-13. 

869 

-12. 

334 


ellipse 1 data: 

x model z measured z 

-20.302 -8.617 -6.491 


ellipse 3 data: (pixel 108-157) 


X 

model z 

measured 

-4.863 

2.932 

1.372 

-5.072 

2.734 

5.160 

-5.282 

2.536 

2.465 

-5.492 

2.335 

1.543 

-5.702 

2.133 

2.537 

-5.912 

1.930 

3.019 

- 6.122 

1.725 

2.882 

-6.331 

1.518 

1.111 

-6.541 

1.310 

-0.004 

-6.751 

1.100 

1.205 

-6.961 

0.888 

1.500 

-7.171 

0.676 

1.920 

-7.380 

0.461 

1.870 

-7.590 

0.245 

1.283 

-7.800 

0.028 

0.221 

-8.010 

-0.191 

-0.444 

-8.220 

-0.412 

0.391 

-8.429 

-0.633 

-0.807 

-8.639 

-0.857 

-0.964 

-8.849 

-1.082 

0.218 

-9.059 

-1.308 

- 1.868 

-9.269 

-1.535 

-1.539 

-9.479 

-1.765 

-2.378 

-9.688 

-1.995 

-3.032 

-9.898 

-2.227 

-2.364 

-10.108 

-2.461 

-1.190 

-10.318 

-2.695 

-3.601 

-10.528 

-2.932 

-2.719 

-10.737 

-3.169 

-3.826 

-10.947 

-3.408 

-3.755 

-11.157 

-3.649 

-3 . 614 

-11.367 

-3.890 

-4 . 976 

-11.577 

-4.133 

-2.961 

-11.786 

-4.378 

-5.399 

-11.996 

-4.624 

-5.273 

-12.206 

-4.871 

-3.103 

-12.416 

-5.120 

-5.650 

-12.626 

-5.369 

-5 .609 

-12.836 

-5.621 

-3 . 914 

-13.045 

-5.873 

-6.682 

-13.255 

-6.127 

-6.652 

-13.465 

-6.383 

-4 .972 

-13.675 

-6.639 

-6.847 

-13.885 

-6.897 

-5.504 

-14.094 

-7.156 

-6.539 

-14.304 

-7.417 

-6.676 

-14.514 

-7 . 679 

-6.620 

-14.724 

-7 . 942 

-6.748 

-14.934 

-8.207 

-10.205 

-15.143 

-8.472 

-7.180 


Table 4.4. A listing of simulation data 
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(b). Smoothed distance curve 


Figure 4.27, Distance curves: (a), theoretical distance curve and cor- 
rupted data points; (b) distance curve after smoothing, (cone 
direction vector = (2,3,4)) 
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Figure 4.28. Distance curves: (a), theoretical distance curve; (b). dis- 
tance curve resulted from measurement errors; (c) distance 
curve after smoothing, (cone direction vector = (0,1,0)) 



CHAPTER 5 


AN OPTIMAL SOLUTION FOR OBJECT LOCALIZATION 

As we have mentioned, two pairs of non-parallel matching line-segments are sufficient to 
completely specify a transformation matrix. In practice, however, it is often the case 
that more than the minimum number of required features can be extracted from sensed 
range data. For example, three or more line-segments may be extracted from sensed range 
data either by using multiple line sensors or by using multiple measurements with each 
measurement displaced slightly. Point features such as the corner points (vertices) of an 
object, masked points, or the center of a sphere may also be extracted from sensed range 
data. 

Techniques which combine redundant sensed features, whether or not they are of the 
same type, to determine the object location can improve the accuracy of localization. 
Least squares optimization techniques are frequently used to find the best estimate of 
the transformation matrix from those redundant features. Two approaches for using 
optimization techniques to find the best estimate of the transformation matrix have been 
introduced [5, 49, 37]. In one method, an optimal orientation of the object is determined 
first, which is then used as a basis to find the position of the object. That is, the translation 
vector is a function of an optimal rotation matrix R and other measured quantities. Many 
object localization algorithms use this method [5, 45, 48, 49, 78, 98]. The problem with 
this approach is the possibility of the existence of the accumulated errors in calculating 
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no 


the translation vector due to the errors from previous calculations and measurements. For 
example, in [5]’s SVD algorithm the translation vector t is computed from R, Pj, P;, e.g., 
t = /(Jl,p.,p,.) where p, and p, are sets of measured points and corresponding modeled 
points in 3-D space respectively. Because both p, and R have errors, the error of the 
resulting translation vector will be compounded due to error propagation. The second 
approach is to compute two optimal solutions separately, one for the orientation and the 
another for the position [37], which is not very efficient. The common characteristics of 
these two methods are that both approach the problem of determination of orientation 
and position separately. That is not surprising, because the transformation matrix itself 
can be easily decomposed into two parts: a rotation submatrix and a position vector. 
The difference between these two approaches lies in the way the optimization is done: the 
first only optimizes the rotational part of the homogeneous transformation matrix and 
the translational part is then derived from it, while the second method optimizes both 

rotational and translational parts separately. 

In this chapter, we present an efficient algorithm which is based on the use of dual 
number quaternions [26]. The method solves for the orientation and the position of an 
object by minimizing a single cost function associated with the sum of the orientation and 
position errors. The performance, both in accuracy and in speed, compared with that 
of the previous methods will be discussed. The required input data for the algorithm is 
a combination of measured points on the surfaces of an object, measured unit direction 
vectors from that object and their corresponding modeled features. Examples of point 
features might be any combinations of those which we have mentioned at the beginning of 
this chapter. Examples of unit vector features include the surface normals, edge direction 
vectors, or axis direction vectors. 
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A brief description of the concept and properties of dual numbers is given in Appendix 
F. More detailed discussion can be found in [105] and [134], In the following sections, we 
begin with the introduction of the definition of dual number quaternions. We show how 
they are used to represent object location, why they are a valid representation of location 
and their correspondence with the more familiar homogeneous transforms. Then we give 
a brief description of the important properties of the dual number quaternions. Next, 
we formulate the object localization problem as a dual number quaternion optimization 
problem and an algorithm is derived to solve the problem. Simulation results are shown 

in section 5.3. 

5.1 Dual Number Quaternions 


This section begins with the definition of dual number quaternions, their properties, 
and their physical interpretation. It concludes by showing how to convert back and forth 
from the dual number quaternion representation of location to the homogeneous transfer- 
mation representation. 


5.1.1 Properties of dual number quaternions 

Quaternions are four element vectors, which are thought of as consisting of a 3 X 1 
vector component and a scalar component. For example the quaternion q is: 


9 = 


9i 
9 2 

93 

94 


q 

94 


(5.1) 


1 

... ' ' 
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SymboHs) 

Description 

Quaternion: 

q } e, a 1 b, r, s, t,n, p, ti*, Pj, , p,* , Pi 

e 

unit quaternion 

t 

translation quaternion 

r 

real part of a dual quaternion 

s 

dual part of a dual quaternion 

n 

direction quaternion 

P 

position quaternion 

n i 

modeled direction quaternion 

P? 

modeled position quaternion 

n, 

transformed model direction quaternion 

P. 

transformed model position quaternion 

rii 

measured direction quaternion 

Pi 

measured position quaternion 

Vector: t, ( 

j, a, n, p, r, Pj, p°, Pi, n? 

t 

translation vector 

n 

rotation axis unit direction vector 

P 

position vector 

P? 

modeled position vector 

Pi 

measured position vector 

P. 

transformed model position vector 

n? 

modeled direction vector 

4x4 matrix: T, I, A, Ci, C 2 , C 3 , Q, W 

T 

homogeneous transformation matrix 

Q W 

quaternion matrices 

3x3 matrix: H, K 

R 

rotation matrix 

K 

skew-symmetric matrix 

Scalar: 0, 

d, e , A t , A 2 1 Qf* i Pi 

0 

rotation angle 

<f 

distance between two vectors 

e 

errors from least squares optimization 

Al, ^7 

Lagrange multipliers 

Of,-, Pi 

weighting factors 

Dual quantities: q f q, n, 0 

Q 

dual quaternion 

n 

dual vector of rotation 

9 

dual angle of rotation 

Table 5.1. A list of symbols appeared in this chapter 
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Figure 5.1. Illustration of rotation for a real quaternion 

In our notation, each quaternion is represented with a boldface script character, such as 
q; and each 3x1 vector is represented with a boldface Roman character, such as q. 

Quaternions have been used extensively as a method of parameterizing orientation 
[52, 68, 94]. 

In this application, the components of the quaternion have the following interpretation. 

sm(0/2)n 

q = 

cos(9/ 2) 

The components of this quaternion are called the Euler Symmetric Parameters. As 
illustrated in Figure (5.1), the vector n is the unit vector about which the coordinate 
system has rotated and 0 is the amount of rotation about n. The corresponding rotation 
matrix R can be expressed as 



R = ( q\ ~ q T q)J + 2qq T + 2q 4 K(q) 


(5.3) 
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where K is the skew-symmetric matrix 

0 <f3 92 

K{q) = q 3 0 -q\ 

-q 2 qi 0 

The extension of this equation to include the representation of position and orientation 
is made by simply changing all of the quantities in the equation to dual quantities [26]. 

91 

9 2 

93 

94 

Note that whether an item is a 3 x 1 vector or a quaternion, if it’s components are 
dual numbers, it is signified by placing a hat over it as in the above example. A brief 
description of the concept of dual numbers and their important properties can be found 

in Section F. 1 of Appendix F. 

There are two parts of a dual quaternion. 

q = r + es (^-®) 

where r and s are both real quaternions and are called the real part and dual part, 
respectively. 

The dual quaternions have a similar interpretation as the real quaternion. 

sin(#/2)n 
cos(0/2) 

where the dual vector n represents a line in 3-D space about which the coordinate system 
has rotated and translated and 0 is the dual angle of rotation and translation. The dual 
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Figure 5.2. Illustration of rotation and translation for a dual quaternion 
vector n and dual angle 0 are 

n = n + cp X n (5.8) 

and 

0 — 9 + ed (5.9) 

where n is a unit vector which specifies the direction of the rotation axis and also the 

direction of translation; the rotation is about the line having direction n passing through 

the point p with a rotation angle of 9 ; and d is the distance of translation along the 

direction specified by n (see Appendix F for the discussion of n and p). The geometrical 

interpretation of the representation can be explained as follows: 

Traditionally, the transformation of a coordinate frame is specified by a trans- 
lation vector t, a rotation axis n and a rotation angle 8. A new coordinate 
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frame is formed by first translating the original coordinate frame along t and 
then rotating it with respect to n by an angle 9. Of course, the sequence of 
translation and rotation can be reversed. 


With dual quaternion representation, the same transformation can be formed 
by first translating the original coordinate frame along the direction of n by a 
distance of d and then rotating it by an angle of 9 with respect to a line having 
a unit vector n as its direction and passing through a point p. 

See Figure 5.2 for an illustration of the interpretation. It can be proven that for each 
(n,p,d,0) transformation representation, we can always find a unique corresponding 
(t, n ,0). On the other hand, for each (t,n,0) transformation representation, there ex- 
ists a set of corresponding (n,p,d,0)’s. (See section 5.1.2 and appendix F for a detailed 

description). 

If we place Eq.(5.8) and (5.9), e.g, expressions for n and 9, into Eq.(5.7), expand and 
simplify that equation by using the properties of dual numbers, and compare the results 
with Eq.(5.6), we have the following equations: 


sin(0/2)n 

cos(0/2) 


(5.10) 


and 


s = 


d/2 cos(#/2)n + sin(0/2)(p X n) 
-d/ 2sin(0/2) 


(5.11) 


A dual quaternion has eight elements whereas the minimum number of independent 
variables to represent a 3-D object transformation is six, which means that two of the 
eight elements in dual quaternion representation are not independent. In fact, it can be 
shown from Eq. (5.10) and (5.11) that the components of any dual quaternion, if they are 
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Q(a) T Q(a) = Q(a)Q(a) T = a T aI 
W{a) T W{a) = W(a)W(a) T = a T aI 
Q(a)b = W{b) a 
Q{a) T a = W{a) T a = ( a T a)e 
Q(Q(a)b) = Q(a)Q(b) 

W(W(a)b) = W(a)W(b) 

Q(a)W (b) T = W(b) T Q(a) 
a and b are arbitrary quaternions 
e is the unit quaternion = [0001] 7 * 

Table 5.2. Properties of quaternion matrices 
defined by Eq. (5.7)-(5.9), satisfy the following two constraints 

r T r = 1 (5.12) 

r T s = 0 (5-13) 


Two important matrix functions of quaternions are the matrices Q{r) and W(r) which 
are defined as: 


Q(r) = 


r 4 I + K( r) r 


— r 


(5.14) 


r 4 I - jRT(r) r 


— r r 4 


W(r) = 


(5.15) 
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where K( r) is the skew-symmetric matrix as was defined in Eg. (5.4). 

Useful properties of the Q and W matrices which are utilized in the derivation of the 
localization algorithm are given in Table (5.2). All these properties can easily be verified 
by direct substitutions. 


5.1.2 Relation to homogeneous transforms 

A common method of representing the position and orientation of a coordinate system is 
with homogeneous transforms. Since homogeneous transforms are more common in use 
than dual number quaternions, the following is provided as a reference to show how to 
convert from one to the other. 

5.1.2. 1 Computing the homogeneous transform given the dual quaternion 

Eq.(5.10) shows that the real part r of the dual quaternion has exactly the same form 
as that defined in Eq.(5.2). As a result, the rotation matrix R can be written in terms of 
the components of the dual quaternion in the following familiar way. 

R = (rl - r T r )I + 2rr r + 2r 4 K(r) (5.16) 


or 


R 0 

0 T 1 


= W(r) T Q(r ) 


(5.17) 


in 


The position vector can be written in terms of the components of the dual quaternion 
the following way (see section F . 2 of Appendix F for the detailed derivation). 


t = W(r) T s 


(5.18) 
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where 


t is ,Ke translation quaternion for the translation vector » and is defined as 


t = 1/2 


t 

0 


(5.19) 


i.1.2.2 Computing the dual quaternion given the homogeneous 


transform 


r T cnpcifipd bv a rotation matrix R and a translation 
Given a homogeneous transform T specitiea oy a roiau 

vector t, one can compute the corresponding r and a as follows: 


( 5 . 20 ) 


r4 = l/2\/ R\\ + R 22 + R 33 d" 1 

where ft, denotes the ij - tk element of the matrix R. A value of r, equal to sero 
represents a rotation of 180 degrees. If r 4 is not zero, then: 

f?32 — #23 

R\3 ~ #31 
#21 — #12 


r = 


4r 4 


( 5 . 21 ) 


If r 4 is zero, then: 


rr T = l/2(i£ + I) 


So r can be determined from any nonzero 


column of 1/2 (R + I), call it a. Thus, 


= ± 


IWI 


(5.22) 


(5.23) 


Either sign will work since the rotation angle is 180 degrees, 

Having determined r, the value of s can be computed from Eq. (5.18) 

s = W(r)t ( 5 ‘ 24 ^ 

Thus a homogeneous transformation matrix and a corresponding dual quaternion can 

be converted from one to another. 
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5.2 Problem Formulation and Solution 

This section begins with the formulation of the problem as an optimization problem. The 
following section presents the solution to the problem. 

5.2.1 Problem formulation 

As we have mentioned, two types of sensor measurements are considered: the position 
of points on an object and the unit vector on the object such as the unit normal, edge 
direction vector, etc.. To facilitate the analysis we define quaternion representations of 
these quantities. Let p be the position vector of a point on the object surface. We define 
the position quaternion as 

p= 1/2 P (5.25) 

0 

Let n be a unit vector extracted from the object. We define the direction quaternion as 

" (5.26) 
0 

To determine the position and orientation of an object, we make measurements of k unit 
vectors for which we have a correspondence to the models and store them in the direction 
quaternions, hi. The tilde denotes measured values. Similarly, we make measurements of 
/ points on the object and store them in the position quaternions, p x . 

Corresponding to each measured point p,, there is a database description of that point 
p9 which is described with respect to the object coordinate system. If t and R are the 
translational and rotational parts of the transformation matrix which is to be determined, 
the modeled point will be transformed into position p, 



p- = t + Rp°i 


(5.27) 
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If these modeled points are represented by position quaternions p ® and Pj and a dual 
quaternion is used to represent the transformation parameters, from Eq. (5.17) and (5.18), 
Eq. (5.27) will become the following: 

p t = W(r) T s + W(r) T Q(r)p? (5.28) 

For the same reason, for the modeled direction quaternion ti,* and we have the relation: 

n, = W{r) T Q{r)n ? (5.29) 

The approach for computing the position and orientation of the object is to determine 
r and s which minimizes the error between the p, and p* and the n, and n,*. That is, we 
select the r and s to minimize the following error function: 

E - £ Q '( n « “ ”«') 2 + ~ P') 2 (5.30) 

1=1 1 = 1 

where the a, and /3, are constant positive weighting factors. 

We consider each of these terms individually. 

(n, -n,) 2 = 2(1 - r T Q{n i ) T W(n ( l)r) (5.31) 

(Pi -Pi ) 2 = 3 T s + 2s t ( W(p°) - Q(Pi))r 

-2 r T Q(pi) T W(p°)r + ((p°) T J>° + pfp.) (5.32) 

Thus, the error function can be written as a quadratic function of r and s. 

E = r T C\r + s t C 2 s + s T C 3 r + constant (5.33) 

where 

Cx = -2X>Q(n,) T lV(n?) -2 £ftQ(fc) r W(p?) (5.34) 

i = 1 «=1 

/ 

C 2 = (£>)/ 

«=i 


(5.35) 



(5.36) 


C 3 = 2^ft(W(Pi)-Q(P.)) 

1 = 1 

k l 

constant = 2 ^a, + ^/3 «((p?) 3 'p? + pfPi) 

.=i t=i 

We compute r and s to minimize this error function subject to the constraints: 


(5.37) 


r T r = 1 (5-38) 

s T r = 0 (5-39) 


5.2.2 Problem solution 


The optimal dual number location quaternion is obtained by adjoining the constraint 
equations to the error equation and then minimizing the resulting function without con- 
straints. 

E = r T C x r + s t C 2 s + s T C 3 r + constant 
+ A i(r T r - 1) + A 2 (s T r) 

where At and A 2 are Lagrange multipliers. Taking the partial derivatives gives: 

— = (Ci + C7)r + Cjs + 2Air + A 2 s = 0 (5.40) 

dr 

^ = (C 2 + Cl )s + C 3 r + A 2 r = 0 (5.41) 

03 

Thus, the solution of equations (5.38), (5.39), (5.40), and (5.41), for r and s gives the 
optimal solution for the position and orientation of the object. 

To solve these equations, we begin by solving for A 2 . Multiplying equation (5.41) by r 


and solving for A 2 gives: 
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Since C 3 is skew symmetric: 

A 2 = 0 

We can now solve for s as a function of r from Eg. (5.41), 

s = — (C 2 + Cl) Cj,v 

Substituting equations (5.43) and (5.44) into Eg. (5.40) gives. 

Ar = Ajr 


(5.43) 


(5.44) 


(5.45) 


where 


A = i( Cl(C 2 + Cl) l C 3 - Ci - Cl) 


(5.46) 


Thus, the quaternion r is an eigenvector of the matrix A and A, is the corresponding 
eigenvalue. In general there will be font solutions to this equation. Since A is real 
and symmetric, all of the eigenvalues and eigenvectors are real and the eigenvectors will 
be orthogonal. The desired solution is identified by referring back to the original error 

equation (5.33). 

Multiplying Eg. (5.40) by r T gives: 

r T C t r = 1/2 r T (Ci + Cf )r = - 1/2 s T C 3 r - A t (5-47) 


Multiplying Eg. (5.41) by s T gives: 

s^Cis = 1/2 s t (C 2 + Cl)s = —1/2 s Cj,r 

Substituting these into Eg. (5.33) gives: 

E = constant — Ai 


(5.48) 


(5.49) 


Thus, the error 
positive eigenvalue 


is minimized if we select the eigenvector corresponding to the largest 
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Having computed 7 % we can now substitute back into Eg. (5.44) to obtain s to complete 
the solution for the position and orientation of the object. 

To give a clearer picture of the above derivation process, in the following the optimal 
dual number quaternion localization algorithm, (DQ algorithm), will be summarized. 

From the algorithm we can see that the execution times for steps 2 - 4 are basically 
constant and the execution time for step 1 has a linear relationship to the number of 
measured vectors. Therefore, the algorithm is an 0(n) algorithm in time complexity. 



125 


DQ Localization Algorithm: 

Inputs: a set of k measured points p°, l measured unit vectors nf; the corresponding modeled 
points Pi and vectors n,; as well as weighting factors a, and 3, chosen heuristically 
to reflect the reliability of the data points. 

Output: an estimation of transformation matrix T. 


Step 1 . Compute matrices C\,C? and C 3 : 

C x = -2^a t g(n 1 ) T W(n?)-2^AQ(p i ) T ^(P?) 

1 = 1 1 = 1 

C 2 = (£/?,)/ 

» = 1 

C 3 = 2'£(3 i (W(p° i )-Q(p i )) 

i = 1 

Step 2. Compute the 4x4 symmetric matrix A: 

A = 1/2 (Cj(C 2 + - C, - Cf) 

Step 3. Compute the eigenvector r corresponding to the largest positive eigenvalue of matrix 
A and derive s from r. 


Step 4. Compute the matrix T from s and r. 



Figure 5.3. The object “Feeder . 

5.3 Simulation Results 

To examine the performance of the DQ algorithm - the accuracy and the speed, computer 
simulations have been carried out on a Compaq 386/20 with an 80387-20 math-processor. 
The simulation data are from a modeled “Feeder” (see Fig. 5.3) which has 32 vertices, 50 
surfaces and 48 edges. The size of the “Feeder” is 322 X 84 X 151 units. The S VD algorithm 
is selected as a sample algorithm to compare the accuracy and performance with our DQ 
algorithm. 

Because the SVD algorithm only accepts 3-D points as its inputs, the sole inputs for 
the two algorithms are the sampled points in order to have a fair comparison of their 
accuracy. The algorithms were tested using 5, 10, 20 and 30 points as input data. Each 
of these tests was repeated for 25 different choices of points, e.g., 25 different sets of 5, 10, 
20 and 30 points were run, and each of these was run 20 times with random errors (see 
discussion below) added to the sample values. In our simulation, the required number 
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Number of Point 




Method Used 




Correspondences 

SVD 


Dual Number 



X 

y 

z 

9 

X 

y 

z 

9 

5 

1.434 

3.013 

1.190 

0.147 

0.461 

0.277 

0.509 

0.147 

10 

1.133 

2.373 

0.843 

0.046 

0.133 

0.215 

0.169 

0.046 

20 

0.296 

0.607 

0.254 

0.040 

0.102 

0.187 

0.108 

0.040 

30 

0.171 

0.246 

0.125 

0.037 

0.115 

0.115 

0.087 

0.037 


Table 5.3. Comparison of Standard Deviation of Transformation Param- 
eters 

of 3-D points p? are randomly selected from a “Feeder” ’s vertices at the beginning of 
each trial. The corresponding measured points p< are then generated by first rotating an 
angle of 36° around an axis through the origin with direction vector (3.0, 4.0, 6.0) followed 
by a translation of (7,8,13), and finally by adding to each coordinates of the resulting 
points Gaussian random noise with mean zero and standard deviation of 0.5. These 
measured data and modeled data are used to compute the estimated orientation and 
translation parameters. To simplify the simulation, all the weighting factors a; and ft are 
set to 1. The standard deviations for the resulting orientation and translation parameters 
are calculated from these twenty trials. Table (5.3) lists the simulation results. All the 
algorithms are written in Turbo Pascal. Mathpak 87 subroutine package from Precision 
Plus Software was used to carry out all the matrix computation, as well as SVD and 
eigenvalue calculations. 

From Table (5.3) we see that the two algorithms produce the same rotation errors 
no matter how many points are used during the simulations, which is expected. For the 
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Figure 5.4. Solution times for the DQ algorithm on a Compaq 386/20 
computer. 

translation errors, the DQ algorithm exhibits better performance than the SVD algorithm 
in all the cases. Even in the case of 30 points, which is supposed to provide a good 
estimation, the DQ algorithm provides average accuracy improvement of 40% for the 
translation parameter calculation compared with the SVD algorithm. 

Figure (5.4) shows the solution times. Except for the case of three samples, which 
uses three points - the minimum number of required points, all the samples consist of an 
equal number of points and vectors. From this figure we see that the computation time is 
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approximating linear in the number of samples taken, which confirmed our analysis about 
the algorithm’s time complexity. The computation time increases at about a rate of 1.0 
millisecond/sample when a math-processer is used. 

During simulation, we also find that the SVD algorithm has many crashes. Table 5.4 
provides a partial listing of data sets taken from the vertex set of the “Feeder”, in which 
the SVD algorithm crashes. We have never encountered a crash from DQ algorithm. 

Fig. 5.5, 5.6 and Fig. 5.7 give more complete simulation results for the two algorithms. 
All the simulation processes are exactly the same as that we have described in the begining 
of this section except more noise levels are tested (seven levels in fact), from Sd=0.1 to 
Sd=5.0. In Fig. 5.5, only points are used as the input of the DQ algorithm. Fig. 5.6 
shows the simulation results when the input consists of equal numbers of sample points 
and direction vectors. The purpose of this simulation is to see if there is any improvement 
on the localization accuracy when direction vectors are added as the input to the DQ 
algorithm. Comparing these two figures, we find that when the number of sample points 
is small (below 10 points), adding direction vectors as additional input to the DQ algorithm 
indeed improves the output accuracy; if more that 10 points are used as the input, adding 
direction vectors has no influence on the accuracy of the computation. Fig. 5.7 shows the 
simulation results of the SVD algorithm. Compared with Fig. 5.5, we find that, in most 
cases, DQ algorithm gives better estimation than the SVD algorithm. 
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B«gln the txacuiion of the algorithm; input data SD - 1.5 

SVD algorithm crashed on tha following data set: {loop 4) 

i: 1 -152.400 -42.862 9.525 

1: 2 -152.400 -11.225 127. 475 

i: 3 -152.400 0.000 89.237 


SVD algorithm 

crashed on 

the following 

data 

sat : 

(loop 51 

i: 1 -161.925 

42.862 

127.475 




i: 2 161.925 

-42.862 

142.875 




i: 3 161.925 

-42.862 

-9.525 




SVD algorithm 

crashed on 

the following 

data 

act: 

(loop 7} 

i: 1 152.400 

42.862 

142.875 




i: 2 161.925 

-42.862 

142.875 




i ; 3 -152.400 

42.862 

-9.525 




SVD algorithm 

crashed on 

, the following 

data 

set : 

(leer 

i: 1 152.400 

0.000 

87.568 




i: 2 152.400 

-24.696 

65.318 




i: 3 -152.400 

42.862 

-9.525 




SVD algorithm 

crashed on the following 

data 

set : 

(loop 9) 

i: 1 152.400 

42.862 

-9.525 




i: 2 -161.925 

42.862 

-9.525 




i: 3 -152.400 

0.000 

116.229 




SVD algorithm crashed on 

the following 

data 

set : 

(loop 12) 

i: 1 152.400 

42.862 

-9.525 




i: 2 152.400 

24.696 

85.318 




i: 3 -152.400 

-42.862 

-9.525 




SVD algorithm crashed on 

the following 

data 

set : 

(loop 15) 

i: 1 -152.400 

42.862 

9.525 




i: 2 -152.400 

42.862 

127.475 




i: 3 -152.400 

11.225 

127.475 




SVD algorithm 

crashed on 

the following 

data 

set : 

(loop 19) 

i: 1 152.400 

24.696 

85.318 




i: 2 152.400 

-42.862 

-9.525 




1: 3 161.925 

42.862 

142.875 




SVD algorithm 

crashed on 

the following 

data 

set : 

(loop 20) 

i: 1 152.400 

42.862 

142. B75 




i: 2 -152.400 

-42.862 

-9.525 




i: 3 152.400 

0.000 

87.568 





Begin the execution of the algorithm 
SVD algorithm crashed on the following 
data set <sd - 1.51: 


i : 

1 

-152.400 

-42.862 

127.475 

1: 

2 

152.400 

11.225 

90.114 

i : 

3 

161.925 

-42.862 

-9.525 

i : 

4 

-1(1.925 

-42.862 

127.475 

i : 

5 

152.400 

11.225 

90.814 


Printing tha results: 

Z 2 algorithm 

loop ok oy ox 

2 - 4.487 -11.891 -17.161 


otheta 

-1.090 


SVD algontr. 
| oxl oy 

I o.cco : 


— — 1 
3.000 


SVD algorithm crashed on the following data set: 
i: 1 161.925 -42.062 -9-525 

i- 2 -152.400 -42.062 -9.525 

i: 3 152.400 11.225 99.814 

SVD algorithm crashed on the following data set : 
i- 1 -152.400 0.000 116.229 

1* 2 -152.400 -11.225 127.475 

i: 3 152.400 -42.062 -9.525 

SVD algorithm crashed on the following data set: 
i- 1 152,400 -11.225 98.814 

i- 2 -152.400 -11.225 127.475 

i- 3 -152.400 -24.696 113.979 


(loop 21) 


{ loop 2 31- 


{loop 24) 


SVD algorithm crashed on the following data set: (loop 25) 

i: 1 -152.400 -42.862 -9.525 

i: 2 152.400 -42.862 9.525 

a: 3 152.400 42.862 9.525 

Printing the results: 


oop 

ox 

DQ algorithm 
oy or 

otheta 

SVD algorithm 
| oxl oy 1 

ozl 

othetal 

4 

-21.136 

3.421 

-11.936 

5.752 

| 0.000 

0.000 

0.000 

0.000 

5 

-7.199 

-4 .964 

-13.358 

0.421 

| 0.000 

0.000 

0.000 

0.000 

7 

-7.547 

-8.093 

-13.406 

0.122 

1 0.000 

0.000 

0.000 

0.000 

8 

-5.363 

-11.756 

-11.740 

-2.014 

| 0.000 

0.000 

0.000 

0.000 

9 

-6 . 722 

-9.025 

-12.312 

-0.129 

| 0.000 

0.000 

0.000 

0,000 

12 

-0.418 

-7.235 

-11.562 

-0.207 

) 0.000 

0 . 000 

0.000 

0.000 

15 

-8 . 144 

-0.614 

-10.902 

1.700 

1 0.000 

0.000 

0.000 

0.000 

19 

-7.042 

-9.142 

-11.996 

0.360 

i 0.000 

0.000 

0.000 

0.000 

20 

-5.523 

-6.858 

-13.750 

0.195 

1 0.000 

0.000 

0.000 

0.000 

21 

-6.173 

-0 .294 

-13.271 

-0.048 

1 0.000 

o.coo 

0.000 

C .000 

23 

-6.231 

-13.926 

-15.750 

0.855 

I 0.000 

0.000 

0.000 

0 .000 

24 

-4.194 

-11.968 

-13.139 

-0.477 

1 0.000 

o.coo 

0.000 

0.000 

25 

-7.770 

-7.691 

-13.198 

-0.135 

I 0.000 

0.000 

0.000 

0.000 


Table 5.4. Data sets taken from the vertex set of the “Feeder” in which 
SVD algorithm crashes 
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DQ Algorithm Accuracy (Point Input) 

O sd-o.l A sd-0.2 O sd«0.5 + 

X sd-2.0 • sd“3.0 □ sd-5.0 




Figure 5.5. DQ Algorithm Accuracy Testing Result - Point Input 
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DQ Algorithm Accuracy (Mixed Input) 


O sd-0.1 

□ 

sd-0.2 

A 

sd-0.5 

O sd-1.0 

♦ sd-2.0 

X 

sd-3.0 

• 

sd-5.0 




Figure 5.6. DQ Algorithm Accuracy Testing Result - Mixed Input 
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SVD Algorithm Accuracy 


® sd-0.1 n sd-0.3 A sd»0.5 O $d-1.0 

♦ sd-2.0 X sd-3.0 • sd-5.0 




Figure 5.7. SVD Algorithm Accuracy Testing Result 




CHAPTER 6 


LINE RANGE SENSOR CALIBRATION 


6.1 Internal parameter calibration 

Sensor internal parameter calibration is the process of identifying parameter values as- 
sociated with sensor internal structure. More specifically, the internal calibration is to 
determine the transformation matrix which describes the relationship between the 3-D 
sensor coordinate frame and the 2-D coordinate frame of the image plane. That is, for 
each readings in a sensor array, different sensor structures require different internal pa- 
rameter specifications. 

The triangulation-based point range sensor which is described in Chapter 4 uses 
Eq.(4.1) to calculate the depth value at each position. Therefore, the focal distance d, 
the source receiver separation value h and the twist angle B between the lens and the de- 
tector array are the internal parameters that must be carefully calibrated in order to get 
an accurate measurement. For the line range sensor, because Eq.(4.2) is used to establish 
the relationship between the 3-D coordinates in the sensor frame and their corresponding 
sensor array readings, the angle 6 of field of view and the correct e, value for each pixel * 
are the parameters which need to be calibrated. In our experiments, because a commercial 
product is used, it is reasonable to assume that the internal calibration has been completed 
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Figure 6.1. X-Y-Z Positioner used for sensor calibration test 

before the sensor is delivered and the sensor specifications can be used as the reference 
for meaningful range measurements. During the experiments, however, we noticed that 
the sensor readings do not agree with what the sensor specifications. Therefore, a simple 
test was arranged to test the internal sensor parameters, e.g., the field of view (or the line 
length) and the coefficients of the linear relationship between the sensor detector array 

readings and true distances. 

8.1.1 Test equipment 

The equipment used to perform the tests included an X-Y-Z manual positioner which 
provide translational movement in three degrees of freedom with a position accuracy of 
0.01mm along each direction (see Figure 6.1) and a special-shaped test object. The object 
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Figure 6.2. Calibrate sensor’s angle of field of view and e, readings 

is made of a prism and a cylinder. The cylinder portion of the calibration object is tightly 
mounted on the movable head of the positioner so that the axis of the object is coincident 
with the Z axis of the positioner. The position of the object is so adjusted that the sharp 
edge of the prism of the object is parallel to Y axis of the positioner 

0.1.2 The e, readings 

Experiments have shown that the relationship between the sensor readings and the real 
distance is a Unear one. That is, if e* represents the hi h readings from the detector cell i, 
is its corresponding true depth data and a total number of n readings are taken from 
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Figure 6.3. A frame of range image to compute the angle of field of view, 
different distances, we have the following set of linear equations for each i G [1,63] 

Xi k = a,e u + bi 1 < k < n. (6-1) 

A least squares fit can be used to compute each a,' and b % . 

In real experiments, the sensor is placed in such a way that its yz plane is coincident 
with the yz plane of the positioner and the x axes of the two coordinate systems are 
parallel. The prism mounted on the positioner is moved along the 2 direction (see Fig.6.2). 
As the prism moves, the readings and its corresponding distance values are recorded and 
Eq.(6.1) is used to compute the coefficients. 

6.1.3 The angle of field of view 

The same experimental set-up can also be used to estimate the line length. The line 
length can be calculated by using the properties of trigonometric functions. Fig.6.3 is a 
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frame of range image 


obtained during the measurement process as described in section 
6.1.2. The line length r ( can be derived by using the following formula: 

X,- = 2 * tan 30° * Az,- 


( 6 - 2 ) 


where ; represents the ith frame of measurement and 

Az, = Z U - Z 32 , = ( a l e l. + “ (° 32 e 32 . + & 32 ) 

The final value of line length can be computed by averaging 11 .he „ V Using the simp.e 
tes, procedure, the computed line length is 1.722mm, which is quite different from the 
specification (2.0mm). The corresponding angles of field of view are rif and 3°4f>\ This 


modified value has been used in feature 


extraction experiments and the correctness of the 


result has been confirmed from these experiments. 


6.2 External Parameter Calibration 


Usually range sensors are mounted somewhere on the robot to make measurements of 
the surrounding objects. To make sensor information useful for the robot, the relative 
position between the sensor coordinate frame and the coordinate frame of the robot must 
be calibrated, which is called the external calibration problem. 


6 . 2.1 Related coordinate frames and transformation matrices 

,u order to describe various calibration procedures, in the following a list of definitions 

of useful coordinate frames and transformation matrices is given: 

e TTcn all v it is assumed to be attached to the base of the 

W : The world coordinate frame. Usually it is assumeu 

robot, and is always fixed no matter how the robot arm moves. 
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S : The line sensor coordinate frame. By convention, the z axis coincides with the optical 
axis of the sensor and the x axis is parallel to the detector array. 

0 : The calibration object coordinate frame. This coordinate frame is used to give a 
complete geometric specification of the object so that the coordinates of every point 
on the object surface are known a priori relative to this frame. 

G : The Gripper coordinate frame. Usually the gripper is installed on the last link of the 
robot arm. 

Tq : The transformation matrix from 0 to W . Its rotation submatrix and translation 
vector are R q and respectively. 

T 5 : The transformation matrix from S to G . Its rotation submatrix and translation 
vector are iZf and respectively. 

: The transformation matrix from gripper frame G{ to W. Its rotation submatrix 
and translation vector are R ^ and p^ respectively. 

Tq : The transformation matrix from 0 to sensor frame 5 t *. Its rotation submatrix and 
translation vector are Rq and p 0 * respectively. 

: The transformation matrix of the gripper frame G 3 to gripper frame G t . Its rotation 
submatrix and translation vector are and p G * respectively. 

T|‘ : The transformation matrix of the sensor frame Sj to sensor frame Its rotation 
submatrix and translation vector are jR|‘ and pf^ respectively. 
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Tim* j 

Tim* I 

Gripper 



Figure 6.4. Illustration of the relationships among coordinate frames dur- 
ing calibration 

.2.2 Basic relationships among coordinate frames 

A, illustrated in Fig.(6.4), the W, G, S and 0 frames have the following relationships: 
L Fixed transformation matrices : 

The relative positions between the pair W and O as well as the pair G and S are 
Sxed during calibration process. Therefore, the transformation matrices T% and 
T 0 do no t change. The objective of the external calibration is to determine T?, 
e.g„ the transformation matrix of the sensor coordinate frame with respect to the 

gripper coordinate frame. 


2. Computable transformation matrices: 
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The transformation matrices (T^)’s are computable matrices during calibration. 
The transformation matrix T™. can be obtained by the robot controller from the 
readings of the joint encoder values and the computation of a chain of forward 
kinematic transformations along these joints. The matrices of (Tq)’s will also be 
available, if the line sensor can determine the location of the object with respect to 
the sensor frame by using only one frame of measurement, such as is the case when 
the line sensor uses a grid of lines, or multiple lines to make a measurement and two 
line-segments features can be extracted from a single frame of line range image. 

3. Derivable transformation matrices: 

The transformation matrices and can be directly derived from the obser- 
vations at two different locations 

T% = [T%]- l T” (6.4) 

and 

Tfj. = T s S[T S 0 T l (6.5) 

4. The basic transformation equation: 

As the robot arm moves from one position to another, the following transformation 
chain holds and is the basis to compute Tf: 

To = T%T§T§ (6.6) 

As we will see, to accomplish external calibration, the sensor has to take necessary 
measurements on a calibration object and make corresponding computation. The degree 
of difficulty in solving this problem will be different if external conditions are varying. In 
the following, we will discuss calibration procedures for three different situations: 
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1 . The object is pieced in a pre-detennined position and the sensor can locate the 
object from a single frame of measurement; 

2. The object location is unknown and the sensor is still able to locate the object by 
using a single frame of measurement, 

3. The object location is unknown and the sensor can only locate certain features from 
a single frame of measurement, for example locate a position vector from that object 

The calibration is quite trivial in the first case in which the relative location between the 
world coordinate frame and the object frame, T'q , is precisely known. This condition can 
be met if it is possible to place the object accurately in a pre-determined location. In this 
case, the transformation matrix can be easily derived from Eq.^.G) 1 

t g s = ra-^Tgr 1 < 6 - 7 > 

Although this method is very straightforward and requires little computation, it is rarely 
seen in real application because finding the accurate location between the world frame W 
and the object frame 0 is not easy. 

0.2.3 The calibration algorithms with T $ unknown 

In this section, we describe the calibration procedures for a more general case in which 
(1) there are no constraints on the relative location between the object frame and the 

world frame, and (2) the line range sensor is able to locate the calibration object each 

1 An implicit assumption has been made here that the matrices T Gi are available, as it 

is the case in general when the matrix can be obtained from the readings of robot controller’s 

decoder 
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time the robot moves to a particular position, e.g, T§ is known at location t. In this case 
the calibration of the sensor’s position with respect to robot gripper frame is computed 
by displacing the robot and observing the changes in the sensor frame using the sensing 

system. 

Suppose the robot arm moves from position 1 to position 2 (see Figure 6.4). We will 
have two transformation equations, e.g., Eq.(6.6) for i = 1,2. Because the object is fixed 
during the calibration, the following equation can be obtained 


rpW rpGrpS\ _ rpW J^GrpS^ 

T G i 1 S 1 Q ~ 1 Gi 1 S 1 O 


( 6 . 8 ) 


Except for singular points, the transformation matrices are invertible and Eq.(6.8) can be 


changed into the following form 


ATg = TgB 


(6.9) 


from Eq.(6.8), where 


A = [T^V'T” = T G g \ 
B = T&[T§]- l «Tg 


( 6 . 10 ) 

( 6 . 11 ) 


Because A and B are known matrices, the calibration problem can be thought of as a 
problem of solving a homogeneous transformation equation of the form AX = XB [109]. 

Finding the general solutions for the matrix equations of the form AX = XB is not 
an unsolvable problem and it has long been discussed in linear algebra theory [51]. [109] 
also has given a solution for this equation. However, in our application, because rotation 
is involved in the computation which enables us to use many important properties of 
rotation, we can expect to derive a much simpler solution. 

In order to explore the properties of the rotation matrix, we decompose Eq.(6.9) into 
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( 6 . 12 ) 

(6.13) 

(6.14) 


where 

I is the 3 X 3 identity matrix; 

Ra = [Rg\~ 1r Gi = r g\ is the rotation submatrix of A\ 

R B = R s 0 '[R S o']-' = R s\ 5s the rotation subraatrix of - B; 
p A and p fl are the translation parts of A and B. 

From Eq.(6.13) and (6.14), we can observe the following 

is dependent only on rotational matrices Rx and R S . That is, it can be solved 

independently without the knowledge of the translational components of those ho- 

mogeneous transformation matrices. 

2. pg is dependent on R G S . That is, pg can be solved only after R a s is solved. 

3. The key for the sensor calibration is to solve for Eq.(6.13). 

In next section, two algorithms are given to solve for the rotation matrix R a s . Both 
algorithms use the properties of the rotation matrix to make the computation both simple 


and accurate. 
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* G 

6.2.3. 1 Solving for the rotation matrix R s 

Before solving for the rotation matrix R% , we first explore the geometrical interpretations 
of Eq.(6.13). To make our analysis clearer, we rewrite the Eq.(6.13) in a more general 

form R A R — RRb- 

Lemma 6.1 The eigenvalues of a rotation matrix are 1 and cos6±isin6 where 6 is as 
gives below. Let A i = cost + ism6 and A 2 = cos 6- ism 6; then 6 can be calculated by 

6 = arctan (|J?e(Ai — A 2 )|,Ai + A 2 ) (6.15) 

where Re ( Ai - A 2 ) is the real part of X\ - X 2 . 

Proof: See page 412 of [73]. 

Lemma 6.2 Rotation matrices R A and Rg have the same angle of rotation. 

Proof: Because a rotation matrix is always invertible, we have R A = RRbR l . So R A 

and Rg are similar. Similar matrices have the same eigenvalues and thus have the same 

4k 

angle of rotation from Lemma 6.1. 7 

Lemma 6.3 Let n ,4 be the rotation axis of R A and n g the rotation axis of Rg. For any 
matrix R, if R A R = RRb> then n^ = Rng. 

Proof: From given condition that R A R = RRbi we h ave 

R a R n b = RR B ng (6.16) 

Because a rotation matrix always rotates its axis into itself, the right side of Eq.(6.16) 
can be simplified 


R a R n g = R ng 


(6.17) 
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Figure 6.5. Geometrical interpretation of the rotation axis 
Now suppose R nB = n^, then Eq.(6.17) becomes 

R A nk = ^ (6.18) 

Eq.(6.18) means that the vector is an eigenvector of R A with the corresponding 
eigenvalue equal to 1. But only the rotation axis of that rotation matrix has this 
property. Therefore = n^. That is 

R a R n B = R A r\k = n A (6.19) 

Substituting the result of Eq.(6.19) into the left side of Eq.(6.17), we have the required 
equation: 

n A = R n B (6.20) 


# 
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The geometrical interpretation of Lemma 6.3 is that the rotation axis of rotation matrix 
R will rotate n„ into n„. Because a rotation preserves the angle between the transformed 
vector and the rotation axis, and if we define n as the rotation axis of R we will have the 

following relationship: 

n • ne = n • nx < 6 - 21 > 

That is, n • (n„ - n„) = 0, which means that the rotation axis of R is perpendicular to 
(n B - n A ). In fact, we have the following Lemma: 

Lemma 6.4 If n L (n B - n A ) then vector n x (n B + n A ) is parallel to (n B - n A ). 

Proof: Because 

(n B - n A ) (n B + n A ) = n s • n B + n B • n A - n A • n B - n A n A 

= 0 

We can conclude that the vector n B - n A is perpendicular to both the rotation axis n 
and the vector n B + n A . On the other hand, n X (n B + n A ) is also perpendicular to both 
of these two vectors. Therefore, n x (n B + n A ) is parallel to (n B - n A ). # 

Based on the above geometrical analysis, it is not difficult to show that the rotation 
axis always lies in a plane which 

L is perpendicular to the vector (n B - n A ), and 
2. intersects (n b — n^) at its midpoint. 

E.g., the rotation axis has one degree of rotation freedom (n B can be rotated into n A 
about any line in that plane that also goes through the intersection of n A and n B - see 
Figure6.5). That is, one pair of R A and R B defines a plane in which the rotation axis 
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must lie in. As a consequence, at least two distinct pairs of Ra and Rb matrices are 
needed to determine R; and Eq.(4.48), (4.49) and (E.18) will be used to compute the 
rotation matrix. 

From above analysis, two pairs of and R& 1 s are the minimum requirement to 
compute the rotation matrix (in the next section, we will show that this is also the min- 
imum requirement to compute the translation vector). That is, the robot arm has to 
move at least twice from position wq to and then to W 2 to compute the calibration 
parameters. Between u?o and W \ , matrices T a x and T& 1 are obtained; so are T a 2 and 
Tb 7 between w\ and u? 2 - 

To get more accurate calibration results, we usually move the robot arm though a 
series of positions. The final results of the rotation axis and the rotation angle are the 
average values of these observations [45]. 

Suppose the robot moves m times, e.g., from position to then from w\ to W 2 etc., 
until from w m - i to w m . The observed matrices are T^/s and T^.’s where i = 1, • • • m. 
Because two pairs of R f A s and R& are needed to derive one rotation parameters, a total 
number of of m - 1 rotation parameters can be derived from m pairs of Ras and Rb' s. 
The final rotation parameters will be 


and 


n = 


1 

77i — 1 


x> 


( 6 . 22 ) 


M - 1 

*=£ 


1=1 


0 , 

M - 1 


(6.23) 


where n; and are the rotation axis and rotation angle which are derived from the pairs 
of R Ai , Rb, and Rb, +1 matrices. Let n, = [n Xi ,n yi ,n 2i ] T , the standard deviations 
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Input: matrices R ^ and Rq , i 1 • • • m; 

G 

Output: calibration rotation matrix R s . 

Begin 

For i := 1 to m to do 

Compute matrices Ra { and Rb i 

where Ra, — [^G,-i] _1 -^Git 

r b . = ngtug - T 1 ; 

Extract (im., 0.4.) and (n b.^b.) from R Ai and R B ,\ 

For i := 1 to m-1 do 

Use Eq. (4.48) (4.49) and (E.18) to compute n, and 0;; 
Compute final n and 0 by using Eq. (6.23) and (6.22). 

End. 

Figure 6.6. The calibration of rotation parameters: Algorithm 1. 


are 




m — 1 



(nc, - n a ) 2 

m — 1 


a = x,y, z. 


(6.24) 


and 


CB = 




t 

»= i 


0) 2 


m — 1 


(6.25) 


Figure 6.2.3. 1 shows the algorithm to derive the rotation parameters of the matrix R s 

using rotation axis-angle representation (n,0). 

Another algorithm which uses quaternions to represent rotation to compute rotation 
parameters is described in [116] [117]. In addition to the above-mentioned rotation prop- 
erties, one more theorem is derived and enables the algorithm to use the least squares 
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optimization method to compute required parameters. 

Lemma 0.5 If q is the quaternion representation of a rotation, then q has the following 
relationship with and q B 

(qg +q^) x y= = | | 2 q = Vb -<Ia (6.26) 

Proof: See [117). 

The corresponding algorithm is described in Figure 6 . 7 . 

The first algorithm has the advantage of “recursive” computation. That is, each time 
if we take one more observation, the only computation we need is to compute one more 
estimated rotation parameters and average the new estimated results with the old ones. 
All the old results are useful and can be saved for later use. But, this method involves 
trigonometric calculation, which is not very efficient. 

The second algorithm, on the contrary, uses quaternion representation and as a con- 
sequence, trigonometric computation is avoided during the derivation process. Thus, the 
algorithm is very simple and accurate. Least Squares Optimization can be used to solve 
for a system of linear equations as shown in Figure 6.7. The disadvantage of the algorithm 
is its non- “recursive” nature. At each time when a new observation is made, the whole 
computation has to start over again. The old results have no use here. 

6 . 2 . 3 . 2 Solving for the translation vector p^ 

Once the rotation matrix is obtained, Eq.(6.14) can be used to solve for the translation 
vector p^. Again, at least two pairs of equations are needed in order to solve uniquely for 
P 5 . This fact is based on the following Lemma. 
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Input: matrices R^ t and Rq , i := 1 • • • m; 

Output: calibration rotation matrix R s . 

Begin 

01 For i := 1 to m to do 

02 Compute matrices R Ai and Rb, 

03 where R Ai = [RGi-i\~ lR a^ 

04 R Bi = R S S[R S cr l ]- l \ 

05 Extract q A> and q s . from R A , and R B ,; 

06 Solve for a system of linear equations K{ q B . + q^.) • q = q^ - q^ 

by using linear least squares technique 

07 Compute 9: 9 = 2 tan -1 |q'|; 

08 Compute q: q = q / \jt + |q'| 2 ; 

09 Compute q from q and 9\ 

10 Derive rotation matrix R from quaternion q. 

End. 

notice: 

1. K( r) in line 06 is the screw-symmetric matrix 

2. The formula in line 06 uses the fact that K( r) • n = r x n 


Figure 6.7. The calibration of rotation parameters: Algorithm 2. [116] 
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Lemma 6.6 The rank of matrix [R - I } is 2 . 

Proof: Because R has three different eigenvalues (see Lemma 6.1), it is diagonalizable. 


From 


the theory of linear algebra [42], there exists a 3 X 3 matrix E such that 

1 0 0 


R = E 


-l 


(6.27) 


0 \i 0 

0 0 A 2 

where the jth column of the matrix E is the eigenvector e^j = 1,2,3) of corresponding 
eigenvalue. Therefore, 

r 1 0 0 
0 Xi 0 
0 0 A 2 


R- I = E 


-1 


E' 1 - EIE 


(6.28) 


= E 


-1 


00 0 

0 A! - 1 0 

0 0 A a - 1 


(6.29) 


E.g, R- I is similar to 


0 0 0 

0 Ai-1 0 

0 0 a 2 - 1 


00 0 

Matrix 0 Ai - 1 0 

0 0 A 2 - 1 

same rank. Therefore, the rank of matrix R - I is 2. 


, however, has a rank of 2 and similar matrices have the 


# 


The number of unknowns of Eq.(6.14) is 3 whereas the rank of matrix [R A - J] is 2. Thus, 
at least two pairs of independent observations are needed to solve for pg. 
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If more observations are available, a linear least squares optimization equations can be 
set up. 


6.2.4 External calibration of a line range sensor 

Up to now all the calibration algorithms are under the assumption that the sensor is 
able to determine the location of the object by using only one frame of measurement. In 
real applications, this is not always the case. For example, a spot sensor can only give a 
depth reading of a single point at each position. To locate a polyhedral object, at least six 
points on three different surfaces have to be measured in order to determine the location 
of the object [49]. A line range sensor emitting a single striped laser light can only give 
the depth information along a line on the object surface. To locate an object, two line- 
segments have to be extracted. Because such kind of sensors provides less information 
than those we have described earlier, more constraints on the shape of the calibration 
object and on the movement of the robot are expected in order to calibrate such sensors. 
In this section we will introduce an algorithm to calibrate a line range sensor. As we 
described in the previous chapter, a line sensor can extract the parameters of a cone with 
only one measurement along a line on the surface of the cone if the half angle of the cone is 
known and the intersection curve between the line and the surface is an ellipse. It means 
that the position of the cone vertex can be located from a single frame of measurement. 
This important property will be used in the calibration process. 

If we make a series of measurements on the cone and the parameters of the cone can 
be extracted after each measurement, we will have the foUowing equation 


IV rriW rrvG_S, 

Po - 1 G , 1 sPo 


(6.30) 



154 



Figure 6.8. Calibration of a line range sensor 

where pg' is the quaternion representation of the position of the cone vertex with respect 
to the world frame, and pg is the quaternion representation of the position of the cone 

vertex with respect to the sensor frame. 

Because the p% is a constant during calibration, we have 



(6.31) 


Again, Eq.(6.31) can be expressed as 


pW w 

Rg, PG, 
0 1 


jyG n G 

R-S PS 


Po = 


K G, P Gi 


Rs P S 

0 1 


Sj 

Po 


and has the following form 

Jlgjgpg + *%,P s + P S = < R sPo + < p * + p £ 


(6.32) 


(6.33) 


If we restrict the robot arm to translational movement during calibration, the rotation 
matrix R^ t is constant 


D W _ r>W _ jjW 

Rg, — Rg, — *^G 


(6.34) 
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Eq.(6.33) can be simplified 


R% R G sP% + Pg, = R sPo + 


w 


(6.35) 


where Rq is a constant rotation matrix. 

Eq.(6.35) can be further rewritten in the form 

Bg(p§ - P§) = (Kg’)-’(pg; - pg) 


Let p |; = p § - Po' a.nd pg; = (Jlgr'tpg - l*g). » e have 


t>G n S, n ^t 

RsPSj - P G, 


(6.36) 


(6.37) 


The Eq.(6.37) can be interpreted as the following: 

The rotation matrix R § rotates the vector pj into the vvector p %\ . 

Therefore, all the analysis from last section can be applied here and can be used 
the desired transformation matrix. Again, at least two pairs of observations are needed 
to specify the transformation matrix. Thus, the calibration procedure to calibrate a line 
range sensor is basically the same as the general calibration procedures with two more 

constraints (see Figure 6.8): 

1. Constraint on the shape of the object: the object is a cone with a known half angle; 

2. Constraint on the movement of the robot: only translational movement is allowed 


during calibration. 


CHAPTER 7 


SUMMARY AND FUTURE WORK 


7.1 Summary 


7.1.1 Object location determination 

Real-time and high-accuracy are the two primary concerns in many object localization 
applications. To achieve real-time localization, the efforts should focus on the following: 

1) Developing fast sensing techniques. 

2) Reducing the overhead for processing sensed data. 

3) Developing fast algorithms to compute the location parameters. 

To achieve high -accuracy, in addition to developing high quality range sensors, developing 
robust or noise-insensitive localization algorithms is equally important. 

The thesis has discussed different methods and strategies for object localization and 
important issues for fast and accurate localization. The thesis has presented two ob- 
ject localization algorithms and examined the applications of these algorithms in tele- 
manipulation tasks and other manufacturing operations. Both algorithms are fast and 
noise-insensitive. 
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The first algorithm is based on line-segment to line-segment matching, e.g, matching 
modeled line-segment features to the line-segment features extracted from the range data 
which are taken on the surfaces of an object. The line-segment features may be boundary 
edges of a polygon or the intersection of two planar surfaces of an object; or an axis of 
the surface of revolution of an object. The thesis has provided a detailed description 
of the whole localization process including line-segment feature extraction and location 
computation. The emphasis is on the extraction of two types of boundary edges: sharp- 
type edges and rounded- type edges, and the extraction of the axis of a cylindrical surface or 
a conic surface. The feature extraction was performed both on real line range data and the 
simulation of noisy range data. Two sources of noise hve been considered, e.g., the sensor 
measurement noise and the roughness of the object surfaces. During simulation, the sensor 
noise is assumed to have a normal distribution and the roughness of the object surfaces is 
assumed to be uniformly distributed. The feature extraction, particularly axis extraction, 
performed equally well with different noise levels. To reduce the overhead for processing 
sensed range data, the thesis has made every effort to try to develop non-iterative methods 
to solve the computation problems needed during the localization process and has achieved 
some success. Closed form solutions are used to carry out most of computations during 
the localization process. 

The second algorithm is more general. The inputs to the algorithm are not limited 
only to line features. Featured points (point-to-point matching) and featured unit direction 
vectors (vector-to-vector matching) can also be used as the inputs of the algorithm and 
there is no upper limit on the number of the features inputed. The algorithm will allow 
the use of redundant features to find a better solution. The algorithm uses dual number 
quaternions to represent the position and orientation of an object and uses the least 
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squares optimization method to find an optimal solution for the object location. The 
advantage of using this representation is that the method solves for the location estimation 
by minimizing a single cost function associated with the sum of the orientation and position 
errors. Thus it provides better estimation performance, both in accuracy and in speed, 
compared with other similar algorithms. Because the thesis does not deal with the problem 
of how point features and vector features are extracted from range data, only computer 
simulation is used to test the performance of the algorithm. The experimental results 
have indeed displayed a significant improvement in localization accuracy over other similar 

algorithms. 

7.1.2 Object localization technique in tele-autonomous system 

When controlling a remote robot to perform manipulation tasks, an operator has to face 
two problems: time-delays on the signal transmission and the uncertainties of the remote 

environment. 

Prediction display and forward simulation constitute a good method to overcome the 
time-delay problem. In this approach the operator controls a simulation of the remote 
robot without time-delay, with the control signals sent in parallel to the remote system. 
The introduction of time and position desynchronization has further developed the concept 
of predictor display, which allows the operator to desynchronize the time and position 
frames, respectively, of the simulation and the remote robot and further enhance the 
operator’s control ability and flexibility. 

The uncertainty problem usually can be approached from two different objectives: 1) 
developing techniques that help avoid the uncertainty; and 2) developing techniques that 
help the system adapt to the uncertainty. The use of object localization techniques in 
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a remote system to overcome the uncertainty problem is based on the measurement and 

error-removal strategy and thus belongs to the first category. 

The thesis has discussed two cases where the object localization could help. The first 
case is the situation where direct manual control is used to perform a tele-manipulation 
task. The second case is the situation where the remote system has certain degree of 

automation ability. 

To handle both cases, a relative move mode is proposed in order for the remote system 
to use the localization ability to perform tele-manipulation tasks. The basic idea of this 
approach is that the operator will control the simulated robot as usual, but the position 
signals, before being sent to the remote site, are converted to positions relative to the 
object to be manipulated and then transmitted. The remote system is capable of sensing 
the relative position of the object to be manipulated in real-time. It uses this sensed 
information to transform the received relative commands into absolute position commands 
and then proceed to Mow the commanded positions. One of the essential prerequisites 
to implement the above control sequence is the separation of the time frames between the 
local operations and remote system commands. The time-desynchronization has played 
an important rule again. 

7.2 Future Work 


7.2.1 Feature Extraction and Localization Algorithms 

This thesis addressed only limited types of feature extraction problems. Even these prob- 
lems have not been solved completely. The followings are some of the interesting topics 


which are worthy of further research: 
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1. Development of efficient and reliable algorithms to do either elliptical curve or general 
quadric curve fitting from a set of measured range data. Investigation of how the 
shapes of the resulting curve are influenced by the patterns of measurement noises. 

2. Extending the multi-scan axis extraction method to the axis extraction of other 
types of surfaces of revolution or a generalized cylinder. 

3. Study of the method of extracting other types of features, such as point features, 
vector features, curvature features or features which can be used to describe irregular 
surfaces. One example is to find an efficient algorithm to do extraction of sphere 
parameters (the radius and the center of the sphere). 

The two object localization algorithms need to be further studied. In the line-segment 
matching algorithm, a closed form formula is used to compute the rotation and translation. 
In deriving these formulas, it is assumed that the two pairs of line-segments are perfectly 
aligned. In fact, because of various errors, they are not. Therefore, studying the effects of 

their mis-alignment is another research topic. 

In the DQ algorithm, the thesis has not discussed the selection of the weighting factors. 

Intuitively, the weighting factors are closely related to the reliability of corresponding 
matching features. Let us take the extraction of two surface normals as an example. The 
two surface normals extracted from two planar surfaces are used as part of the algorithm’s 
inputs. If one surface is toward the sensor, another surface is near parallel to the sensor’s 
optical axis, it is obvious that the extracted surface normal obtained from the first one 
is more reliable than that of the second one. A quantitative analysis is needed and will 
certainly further improve the accuracy of the algorithm. 
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7.2.2 Object localization in tele-autonomous systems 

The thesis has discussed two situations in which the object localization technique can help 
the completion of tele-manipulation tasks. Because of the lack of adequate sensing devices, 
only a small part of the testing is performed on real data. Simulation testing, however, has 
shown that the technique performs well. The sensor the author used is indeed one with 
a very high resolution, but its line length is too short to collect enough line range data 
in just one frame of measurement. We have found from the literature that some recently 
developed range sensors have characteristics which are very close to our requirements, but 

we have not had time to acquire and use such sensors. 

The thesis has explored only a small number of tele-autonomous system problems. 
Many problems in areas such as part design, part grasping and multi-sensor feedback have 
to be further investigated and resolved, and their solutions have to be applied to tele- 
autonomous systems together with the object localization technique before an efficient 
and workable strategy can be used to implement tele-manipulation tasks. 
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APPENDIX A 


THE FORMULAS USED TO FIND THE PLANAR EQUATIONS 


Suppose the equation of the planar surface to be determined is ax + by + * - r. We 
need to find the a, b and r from a set of k readings (x;,j/;,Zi), where i = 1, • • • ,k and k > 3. 
We have the following equation: 


(A.l) 


p 

f 

• 

xi y\ 1 


1 


-z\ 



a 



X2 1/2 1 




-Z2 



b 


; 



— r 



xjt yk 1 



-z k 


Because the range data are obtained by a line range sensor (or a range image), it 
is reasonable to assume that x, and y x are independent variables with no error and 2, 
is the dependent variable which does contain error. Then for each pair of (x,-,y,), the 
corresponding z value should be 

2 = ax,- + byi - r (A. 2) 

However, the measured z value is z,. The error in the ith measurement is then 

e,- = (ax,- + byi - r) - 2, (A-3) 

By using the least squares method, the parameters a, 6, and d are to be chosen so that 
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the following sum of squares is to be minimized. 

E 2 = £ < c? = £>x, + ** ~ r ~ 2,)2 ( A-4) 

t = l 1 = 1 

Taking derivatives of with respect to a,b and i and set each of the derivatives equal 
to zero, we have the following three equations: 


— — = 2 YYax,- + byi - r - z,)x t - = 0 
= 2 V(ax,- + - r - = 0 

db i = i 

— - = -2 YVaz; + byi - r - z.) = 0 

* fe 

In matrix form, they can be expressed as 


£x< E x «& E x «’ 


a 


E x «' 2 « 

E*.y< E y, 2 Ey« 


6 

= 

E Vi Z ' 

E*< Ejk fc 


-d 


E* 


where the summation is from i = 1 to k, or can be expressed as 



a 


E x « 2 * 

M 

b 

— 

E y« 2 «- 


— T 


E* 


(A.5) 


(A.6) 


for simplicity. 

If the matrix M has an inverse, say W , the solution can be expressed as 


a 


E x t 2 i 

6 

= W 

E y« 2 « 

— r 


. 


(A.7) 
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If the surface normal is represented by 

n — Tty, then 


Tlx 

= a/s 

(A.8) 

Tly 

= b/s 

(A. 9) 

Tlz 

= l/ 5 

(A. 10) 

d 

= rjs 

(A. 11) 


where s = \/a 2 + 6 2 + 1 • 

The plane equation, when expressed by surface normal and distance parameters, will 
be in the following equivalent form: 

n x x + n y y + n z z = d (A.12) 


or 


n • x = d 


(A. 13) 
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appendix b 


CIRCLE PARAMETER DETERMINATION USING LEAST 
SQUARES OPTIMIZATION 


Suppose that the 3-D range data are projected on the plane which is perpendicular to 
the axis of the cylinder and are expressed as 5 = {(z« 5 yi)|* — > ^}- Assume also 

that the desired circle is located at c = (x 0 , yo) with radius r, The area of the circle will be 
7 rr 2 and the measured area from the measured point (x,-,y,) is 7r((x; - x 0 ) 2 + (yi ~ Vo) )• 
The error between the two areas is 

e t - = ir((xi — xo ) 2 + (yi — yo) 2 ) — irr 

and the error function to be minimized is 

E = Y j e] (B.l) 

1=1 

Differentiating Eq.(B.l) with respect to r, x 0 and yo and setting the derivatives equal to 
zero results in 


^ = V[((x, - xo) 2 + (y. - yo) 2 ) - r 2 ]r = 0 

dr ti 

(B.2) 

= y\((xi - x 0 ) 2 + ( Vi - yo) 2 ) - x 2 ](x,- - X 0 ) = 0 
dx 0 fli 

(B.3) 

= T(((xi - x 0 ) 2 + (Vi - yo) 2 ) - r 2 ](y,- - yo) = o 
dy 0 ,tt 

(B.4) 


and 
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respectively. Eq.(B.2) is equivalent to 

£((*< - xo) 2 + {Vi ~ yo) 2 ) = kr 2 (B.5) 

t=l 

Replacing Eq.(B.5) into Eq.(B.3), we have 

5^[((x,- - x 0 ) 2 + (Vi ~ Vo ) 2 ) “ r2 l a; i = 0 ( B - 6 ) 

i=i 

Therefore, ^ 

B(x, - xo) 2 + (K - yo ) 2 )*i = (E x *) r2 ( BJ) 

,=i «=» 

For the s am e reason, replacing Eq.(B.5) into Eq.(B.4) yields 

y^((x, - x 0 ) 2 + ( yi - yo) 2 )yi = (E y^ 2 ( B *®) 

,=i > =1 

To eliminate the r 2 term from Eq.(B.7), we multiply both side of Eq. (B.5) by 5Z»=i x * 
and subtract both side of Eq.(B.7), which is multiplied by k : 

T(( Xi - x 0 ) 2 + (yi - yo) 2 )(E x *') “ k E« Xi “ x °) 2 + ( yi ~ yo ^ Xi = 0 ^ B ’ 9 ^ 

»=i ,=1 ,=1 

Similarly, to eliminate the r 2 term from Eq.(B.8), we multiply both side of Eq. (B.5) by 
y { and subtract both side of Eq.(B.8) which is multiplied by k: 

y({xi - X 0 ) 2 + (yi - yo) 2 )(E^*) - k E(( Xi - x °) 2 + " yo ^ yi = 0 (B - 10) 

,tt i=l *■» 

Next we use the following simplified notation. 

k k k 

2> = E x * E* = Ew E« = E x ** 

1=1 t=l 1=1 

E* 2 = I>? E* 2 = I>, 2 = 

i=i »=i *=i 

e* 3 = E x ? E^ 3 = E x t 3 e^ 2 = E x « z ‘ 2 

i=l i=l i=1 




and the solution for xo and j/o is 


( z xz 3 + 3)* - U z 3 + r* 3X* 3) 


fS M 

M H 


M R 

0 R 


0 « 
+ 


•s M 

cd ^ 

►“* H 
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APPENDIX C 


CONIC AXIS PARAMETER EXTRACTION 


C.l The Intersection Point of the Axis of a Cone and a Plane 

As we have discussed in Chapter 4, a conic surface can be specified by a triple (v, n, A), 
where v is the vertex of the cone, n is a unit vector which defines the direction of the axis 
of the cone, and A = cosB where 6 is the half angle of the cone. A point x = (x, y, z) is on 
the conic surface if and only if the vector v • x makes the angle 9 with n or -n. We have 
the following general form of a conic equation: 

|(x - v) • n| = A|x - v| • |n| 

The above can be written as 

[m(x - iq) + n 2 {y - v 2 ) + n 3 (z - u 3 )] 2 = A 2 [(x - v x ) 2 + (y - w 2 ) 2 + 0 “ «3) 2 ] t 0 ’ 1 ) 

The line equation which is coincident with the axis of the cone can be expressed as 

{ i— vi njL 

y-v2 n 2 (C.2) 

z — vi Hi 

y-v2 ““ 712 

Without losing generality, the plane y = 0 is selected as the intersection plane in order 
to simplify the discussion. The intersection of the cone with the plane will produce the 
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following quadratic equation: 

[m(x - vi ) - n 2 v 2 + n 3 (z - u 3 )] 2 = A 2 [(x - ui) 2 + v\ + (z - u 3 ) 2 ] (C.3) 

which has a general form 

Ax 2 + Bxz + Cz 2 + Dx + Ez + F = 0 (C.4) 

The coordinates of the intersection point of the axis line of the cone with the plane 
y = 0 are 

From Eq.(C.3) and Eq.(C.5) we can make the following observations: 

• The changes of tq and u 3 merely translate the conic section on the plane. 

• The effect of changing n x and n 3 on the conic section will be a rotation in the 

zz-plane. 

• As we change iq,u 3 ,ni and n 3 , the position of the intersection point of the cone 
with the plane will also change. 

In summary, the changes of parameters vi,v 3 ,n x and n 3 will only affect the position 
and orientation of the intersection curve and will have no affect on the shape of the 
quadratic curve (conic section) intersected by the plane. 

Now let us set v 3 = 0 and n 3 = 0. This corresponds to assuming that the vertex of the 
cone is in the yz-plane and the axis of the cone is in the plane also. When these values 
are substituted into Eq.(C.3), the equation of the conic becomes 

(A 2 - n\)x 2 - 2 [tq(A 2 - nl) - + A 2 z 2 

= 2n l n 2 v x vi - u?( A 2 - n\) - u|(A 2 - n^). 
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and the position of the intersection point of the axis with the plane wiU then become 

ft - 
0 
0 

From this equation we can conclude that if the quadratic intersection curve is an ellipse, 
the intersection point of the axis of a conic surface with the plane is always on the principal 

axis of the ellipse. 

We can further simplify Eq.(C.6) by properly translating the conic section aiong the 
2 axis. To do that, we can set p 2 = A 2 - n? and », = n.-w/p 2 . The resulting conic 
equation then reduces to 

pV + aV = Jfc 2 ( c - 8 ) 

in the case 0 < A 2 — n\ < A 2 . 

That is, the right-hand side of the Eq.(C.6) reduces to a positive constant k\ To see 
that this is correct we only need to observe that the left-hand side of Eq.(C.6) reduces to 
the left-hand side of Eq.(C.8), which is positive for any (*,*). Hence the right-hand side 
of Eq.(C.6) must be a positive constant. 

After simple arithmetic manipulation, the expression for x in Eq.(C.7) can be simplified 
as 

x = t> 2 «l(l _ ^ 2 ) (C-9) 

which will not be equal to zero because the half angle d of a cone can never be zero. 
Eq.(C.9) tells us that if the curve of the intersection between a plane and a cone is an 
ellipse, the intersection point of the axis of a cone and a plane can never be coincident 
with the center of the ellipse. Eq.(C.9) also gives us the amount of translation from the 
center of the ellipse along the principal axis of the ellipse. 
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Figure C.l. Cone Axis Parameter Extraction from a Line Sensing System 

C.2 The Cone Axis Parameter Extraction 

The discussion in last section will be very helpful in solving the axis parameter extrac- 
tion problem. Suppose a cone is sensed by a laser line range sensor. The sensor coordinate 
system is shown in Fig. C.l. The measurements are made along the X axis of the coor- 
dinate system. The depth data are expressed as Z values. That is, the measurements are 
taken in the y = 0 plane. After the measurements, ellipse parameters can be extracted 

and the ellipse can be expressed as 

Ax 2 + Bxz + Cz 2 + Dx + Ez + F = 0 (C.10) 

On the other hand, the intersection of a cone with the y = 0 plane is given in Eq.(C.3). 
Comparing the corresponding terms of the above two equations, we have a set of six 



173 


equations with six unknown: 


A 2 - nj = A 


-271x713 = B 


1 A - 7l 3 - O (C.ll) 

2ni(n 2 v 2 + U 3 U 3 ) — 2v\A = D 
2n 3 (n 2 v 2 + nivi) — 2t ) 3 C = E 
A 2 (v 2 + r 2 + u|) - (tiiVi + 7i 2 V2 + 713 W 3) 2 = F 
The n can be solved from equations for A, B and C. If B ^ 0, both n x and n 3 must 
be non-zero. In this case, we can derive n x from the equation for A: 


n t = ±yJiA - A 2 ) 


(C.12) 


and then derive ti 3 from the equation for B : 


B 


«3 = - 


2tii 


(C.13) 


712 can be derived from nx and ti 3 : 

772 = ±\f ( 1 - «l - ^ C ' 14 ^ 

Once the cone’s axis direction is found, the location of the cone’s vertex can be found too 
from the remained equations for D, E and F. From equations of D and E, we have 

27 iin 3 t; 3 - 2Av\ = D - 2n x n 2 v 2 (C.15) 


and 


- 2Cu 3 - 2 Tij ti 3 v \ — E - 2n 2 n 3 v 2 


(C.16) 
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Therefore 


V3 


D - 2niTi2V2 -2 A 

E — 2n.2n.3V2 —2n\U3 


= f V3 (v 2) = 03^2 + fo 


2n\U3 -2 A 

-2 C -2n\U3 


(C.17) 


Ui 


2nin3 D — 2nin 2 V2 
—2 C E — 2xi2Xl3 v 2 
2nin 3 —2 A 


f vi (v 2) = a x v 2 + bi 


(C.18) 


| -2C -2nin 3 j 

where coefficients 01,03 and 61,63 are now known quantities. 

Substituting Eqs.(C.17) and (C.18) into the equation for F, we can solve for v 2 with 
two solutions. The tq and t> 3 can then be solved by substituting v 2 back to Eq.(C.17) and 


(C.18). 

From above discussion we know that there are usually eight possible solutions for the 
location of the cone from a given ellipse equation. But only one is correct. The correct 
one can be identified by two methods: 


• Take more measurements. Each additional measurement can give us one more so- 
lution set. We can compare these different solution sets and find the one which is 

common or close to common among all the solutions. 

• The approximate location of the cone is supposely known. We can compare the set 
of solution with the supposed location of the cone’s axis and take the one which is 
closest to the supposed solution as the correct one. 


In fact, in practice, the two methods should be combined to find the correct answer 



APPENDIX D 


DATA SMOOTHING METHODS 


D.l Quadric Curve Fitting 


The measurement data are a set of discrete points on a coordinate plane, which can be 
expressed as 


S = {(xi,z;)| t = 1, •••,*} 


where in some cases, x.’s are integers. In order to obtain a smoothed curve from these 
measured data, we first make observations on the data to see what kind of curve is a 
best fit. We then try to find the parameters which define that curve to give a better 
approximation. Usually, a least squares optimization is employed, which minimizes the 
sum of the squares of the difference between each measured value and the corresponding 
value from the smoothed curve. 

If a quadric curve is a suitable approximation to the set of measurement data, the 
curve can be expressed as 

z = a + bx + cx 2 (D«l) 


We will minimize the following error function 
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Differentiating Eq.(D.2) with respect to a, b and c. and setting them to zero, we obtain a 
system of three linear equations with three unknown: 

\ £?=i z t =a + b(\ £? Xi) + c(| 

< £=! * = a + Klh S*' 1 *•) + c (iCT ^ ) (D ‘ 3) 

HiZi zi = a + Ktt 2 x< ) + 53*=i 

A closed form formula can be found for the solution of these equations. The extreme 
value for z and its corresponding position x can be obtained from the resulting quadric 

equation. 

D.2 Data Smoothing 

It may be difficult to use only one curve, whether a quadric or a polynomial of higher 
order, to fit the entire set of measured data. An alternative method is to use piece-wise 
smoothing. In this method the smoothed value z, at position X{ is dependent only upon 
its surrounding z values. If the z, value depends on z values at positions i - 2, t - 1 , i, 
i + 1 and i + 2, it is called five-point smoothing. If the z; value depends on z values at 
positions from i - 3 to i + 3, it is called seven-point smoothing. 

Now, suppose we have the following set of measured data. 


X 

x 0 x\ = x 0 + Ax 

• • • Xi = x 0 + iAx • ■ 

■ ■ x m - x 0 + rnAx 

z 

yo yi 

Vi 

2/m 
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If we let t = the above table becomes 


t 

-i -(t- 1 ) 

■ ■ -1 0 

1 

(m — i — l) rn — i 

Vi+t 

yo yi 

■ • y.-i yi 

y.+i 

* ’ * 2/m — * 1 Vm 


If a quadric curve is used to fit each section of piece-wise data, the smoothed z value 
at position i + t is 

z,+t = a + bt + ct 2 (D.4) 

where coefficients a , 6, c should make 

^((a + bt + ct 2 ) - z i+t ) 2 minimum (D-5) 

t 

The sum is over (2 n - l) of z values with the t's being closest to i within the interval of 
[0,m]. If n = 2, it is five-point smoothing; if n = 3, it is seven-point smoothing. 

When i - n > 0 and i + n < m are satisfied at the same time, the a, b and c can be 
solved using the following system of equations: 

(2n + l)a + 2(*j I 5 h‘g)c =: n Zi+t 

■ 2(^ + 4 + |)‘=E"=-nl*i+. (D ' 6) 

. 2(4 + 4 + f ) a + 2 (4 + 4 + 4 - »> c = n ‘ 2z( *‘ 

For n = 2 we have the following solutions: 

Zi = ^[-3(z ,- 2 + Zi+ 2 ) + 12(zi_i + 2 .+ 1 ) + 17z t ] (t = 2,3, • • • , m - 2) 
zq = ^(31zo + 9zi — 3z 2 — ^23 + 3 ^ 4 ) 

< z\ = 35(920 + 13zi + 12z 2 + 6^3 — bz4) ^ 

Z m -1 = ^(-5z m -4 + 6z m -3 + 12z m _ 2 + 13z m _l + 9z m ) 
z m _ 2 = ^(3z m _4 - 5z m _3 — 3z m _ 2 + 9z m _i + 3 1 z m ) 


V. 
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If n 
to i. 


= 3, the value z, at position i will depend on the values of the seven positions nearest 
The formula can be obtained from the same system of equations in Eq.(D.6). 
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APPENDIX E 


ROTATION PARAMETER DETERMINATION 


E.l About Eq. 4.48 

The derivation of Eq.(4.48) is based on the fact that any rotation will preserve the 
angle between the transformed vector and the rotation axis (see Fig. E.l.(a)). That is, if 
vector n, is rotated into vector n, and r is the rotation axis, we have 

r • n, = r • n< (E- 1 ) 

or, equivalently, 

r • (m — n.) = 0 ( E - 2 ) 

That is, r is perpendicular to (n, - n,) both for i = 1 and i = 2. Hence r is given by 

_ (ni - nQ x (n 2 - n 2 ) 
r - ||(ni - ni) X (n 2 - n 2 )|| 


to within an ambiguity of degree of 7r. 
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(b) 


Figure E.l. (a), vector n; rotates with respect to vector r into vector n,; 
(b). the vectors projected on the circle plane. 

E.2 About the angle of rotation 


Consider the unit vector hi rotated by an angle 6 into the vector n,; the rotation takes 
place along the arc of a circle in the plane perpendicular to r (see Fig. E.l). Let a be the 
projection vector from hi onto r, We have 

a = (hi • r)r (E-4) 

with b and c as vectors in the circle plane designed in such a way that 

n, = a + b + c (E-5) 

If a is the angle between r and h, then the radius h of the circle is given by 
h = sina|h,j = - — . = |r x n t j, (|r| = 1). 

I r l 


(E.6) 



181 


Furthermore 

b = (n, -r)cos0 and c = (rx hi) sin# (E.7) 

We obtain the following 

n, = a + cos #(h,- - a) + c 

= cos #n,- + ( 1 — cos #)a 4- sin #r x n f - 

= cos Ohi + (1 — cos 0)(r • hi)r + sin Or x hi (E.8) 


Now, “-fij” the two sides of Eq.(E.8), we have 

n, • h, = cos#h, • h, + (1 - cos#)(r • hi)r • h, + sin#(r x n.) • h, (E.9) 


Eq.(E.9) can be reduced to 

n, • h, = cosO + (1 — cos#)(r • h,)r • hi 

= cos 0 4- (1 - cos #)(r • ri,-) 2 (E.10) 


Hence 


n, • h, - (r • h,) 2 


cos 0 — 


(E.ll) 


l-Cr-fti ) 2 

From cos 6, we can derive sin#. Using the similar procedure, e.g., first n, the two 
sides of Eq.(E.8), we have 

n, • n, = cos Ohi • n, + (1 - cos#)(r • n,)r • n< + sin #( r X n.) • n, (E.12) 


Because r • h, = r • n;, Eq. (E.12) becomes 

1 = cos#h, • iii + (1 - cos#)(r • hi) 2 + sin#(r X n;) • n, (E.13) 

Combining the terms for cos 0, we obtain the new equation 

1 _ ( r • hi) 2 = cos #(hi • m — (r • hi) 2 ) + sin #(r x hi) • ni (E.14) 
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If fii is not parallel to r, then 


„ n« • n, - (r n,) 2 , (rxn^-n, 

1 = cos# — 7 tt - 1- sin#- - —r 

1 — (r • n,) 2 1 — (r • n,) 2 

Substituting Eq.(4.49) into Eq.(E.15), we obtain 

r x 

1 - (r • n, ) 2 


~ . „(r x n^) • n, 

1 = cos 9 2 + sin #7 


That is 


Therefore 


. 7 /i . >x n t )n, 
sin 6 = sin 6 - 


sin# = 


1 - (r • n,) 


(r x np • n, 
1 — (r • n,) 2 


(E.15) 


(E.16) 


(E.17) 


(E.18) 


E.3 About the rotation matrix 

If we define a rotation operator R( r,#), Eq.(E.8) can be rewritten in the following form: 

n, = fl(r,#)n t - (E.19) 

Because (r • n,)r = (rr T )n, and r x n, = K(r)n { , the operator will have the following 
form: 

R(r,6) = cos #J + (1 - cos#)rr T + sin9K(r) (E.20) 


Let r = (r x ,r y , r z ), Then 


R — cos 6 

1 0 0 
0 1 0 

+ (1 — COS#) 

r\ r x r y r x r z 
r y r x r 2 r y r z 

+ sin 9 

0 —r z r y 

t z 0 -r x 


0 0 1 


r 2 r x r z r y r 2 


1 

O 

H 

f- 

1 


n 
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APPENDIX F 


DUAL QUATERNIONS AND THEIR APPLICATIONS IN 
REPRESENTING POSITION AND ORIENTATION 


F.l The concept and properties of dual numbers 

A dual number a = a + eb can be defined as a combination of two ordered real numbers a 
and b with a special multiplication rule for e given by e 2 = 0. The two real numbers a and 
b can be said to belong to the real part and the dual part of the dual number, respectively. 
Addition, subtraction, and multiplication of dual numbers are defined by the formulae: 

(a + (b) + (c + (d) = (a + c) + f ( b + d ) 

(a -I- (b) - (c + ed) = (a - c) + e(b - d) (F- 1 ) 

(a + fi>)(c + (d) = ac + ((ad + be) 

Dual numbers were first considered by the famous German geometer E. Study (1862 
- 1930) in the beginning of this century [105]. In his study he used the dual number to 
represent the dual angle which measures the relative position of two skew lines in space. 
That is, a dual angle was defined as 


0 ~ 0 cd 


(F.2) 
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where the d is a distance between two lines in three-dimensional space and the 9 is the 
angle between their directions. 

Some important properties of dual numbers are 

1. The product of a dual number a and its conjugate a = a — eb is 

ha = a 2 (F.3) 

2. The modulus of a dual number 

|a| = a (F.4) 

which can be negative. 

3. Due to the fact that e 2 = 0, the dual number function has a very simple form of 

Taylor series expansion: 

f(a + eb) = f(a) + ebf'(a) (F .5) 

4. For the dual angle 6, we have 

sin (0) = sin(0 + ed) = sin(0) -f ed cos(0) (F*6) 

and 

cos(0) = cos (9 + ed) = cos{9) — edsin(9) (F.7) 

In the above, the properties (F.3) and (F.4) can be derived directly from the definition 
of multiplication of two dual numbers. To show property (F.5), we only need to know that 
a function of a dual number f(a + eb ), like the usual functions over the field of complex 
numbers, can be expanded into a formal Taylor series. That is, according to the theorem 
of Taylor series expansion, if a function f(a) is analytic within a circle \a — c\ < R (R > 0) 
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where c is the center 
that circle 


of the circle, then / (d) can be expended into a Taylor series within 


/(d) = £ «»(d - 0" (F ' 8) 

n=0 

where „ n = and the series is unique. If we expend /(d) at point a, the Taylor series 


will have the form 


6 2 c»r 


f(a + «6) = /(a) + ebf (a) + e 2 — f (a) + • • • 


(F.9) 


Because = 0, all the terms with the power of € greater than one in the Taylor series Will 
be tero. To get the last property, we simply expand sin(d) and cos(d) into its corresponding 

Taylor series at 9. 

The idea of dual quantities can be extended to define dual vectors, dual quaternions, 
and dual matrices. These dual quantities enable two different quantities to be combined 
into one in many ways. For example, a dual vector can be defined to form any line in 3-D 
space. The direction and position of the line can be specified as follows: 


where n is a unit direction vector of the line and p is a position vector of any point 
on the fine. As an another example, a dual quaternion can be defined to represent any 
transformation between two coordinate frames which has been shown in Eq.(5.7). 
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F.2 Position vector expressed in terms of the components of the dual 
quaternion 

The derivation of Eq. (5.18) t = W(r) T s 

Consider an object coordinate system which is transformed into a new location, where 
the transformation is done by first translating the coordinate system in the direction of 
the unit vector n by a distance d (Fig.F.l (a)) and then rotating it by an angle 6 with 
respect to a line having a unit vector n as its direction and passing through a point p 
(Fig.F.l). The position vector p as shown in Fig.F.l will be transformed into vector p'. 
The translation vector t can thus be expressed as 

t = p + dn - p* 

= p + dn — Rp 

= (I - R)p + dn (F.l 1) 

The rotation matrix R , when defined by a rotation angle 6 and a rotation axis n, has 
the form (see Eq.(E.20): 


R = cos0J + (1 - cos0)nn T + sintf-K^n) (F.12) 

where the K( n) is the often mentioned skew- symmetric matrix. 

Because nn T = I + /f(n)itr(n), Eq. (F.12) can be changed into 

R = ca&OI + (l-cosO)(I + K{n)K(n)) + smOK{n) 

= I + (l-cos6)K(n)K(n) + smdK{n) 

= I + 2sin 2 (9/2)K(n)K(n) + sin0.Ff(n) 


(F.13) 
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tem 


Replacing Eq. (F.13) into Eq. (F.ll), we have 

t = (—2 sin 2 (9/2)K(n)K(n)p — sin0ff(n))p + dn 

= 2 sin 2 (0/2)n x (p X n) + sinfl(p X n) + dn 

On the other hand, from Eq. (5.10) and (5.11) we have 

r 4 s-s 4 r = (d/2)n + sin(0/2)cos(0/2)px n 

= (dn + sin0p x n)/2 


and 

r x s = [sin (0/2)n] x [d/2 cos (#/2)n + sin (0/2)p x n] 
= sin 2 (0/2)n x (p x n) 


because n x n = 0. Therefore 


(F.14) 


(F.15) 


(F.16) 


t = 2(r 4 s - s 4 r + r X s). 


(F.17) 
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On the other hand, 


W(rfs = 


r 4 I + K(r) -r 
t t r 4 

(r 4 J + K(r))s - s 4 r 


54 


r T s 


r 4 s — s 4 r + K(r) s 

0 


l/2t 

0 


If we define the translation quaternion t for the translation vector t as 

t 


t = 1/2 


0 


then we have the formula 


t = W(r) T s 


(F.18) 


(F.19) 


F.3 Finding a transformation tuple (n, 0, d, p) from a given dual quaternion 

Given a dual quaternion, the rotation submatrix can be derived as previously described, 
and the rotation axis n and angle 0 can be extracted from the matrix. The translation 
distance d can be derived from Eq.(5.11) 

2s 4 


d = 


sin (0/2) 

Eq.(5.11) can also be used to derive p. From Eq.(5.11) we have 

s = d/2 cos (0/2)n + sin (6/ 2)(p X n) 


This equation can be written as 


p x n 


s - rf/2cos(fl/2)n 
sin (0/2) 


(F.20) 


(F.21) 


(F.22) 
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Because p X n = K{— n)p, we have 

s — d/2 cos (0/2)n 
n)p: — — 

where the only unknown is p. 

The rank of matrix K(-n) is 2 and thus the dimension of null space of the matrix is 
L Eq.(F.23) has the a general solution in the following form: 

P = Po + « n (F.24) 

where p 0 is any vector in null space. 

That is, the solution for p is not unique. Usually, we select one of the vectors which 
axe perpendicular to n as the desired solution. 

One possible solution is 




(F.25) 


where n\ / 0. 
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